JP7024215B2 - Mobile control device and control system - Google Patents
Mobile control device and control system Download PDFInfo
- Publication number
- JP7024215B2 JP7024215B2 JP2017113380A JP2017113380A JP7024215B2 JP 7024215 B2 JP7024215 B2 JP 7024215B2 JP 2017113380 A JP2017113380 A JP 2017113380A JP 2017113380 A JP2017113380 A JP 2017113380A JP 7024215 B2 JP7024215 B2 JP 7024215B2
- Authority
- JP
- Japan
- Prior art keywords
- value
- control value
- obstacle
- moving body
- control
- 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
- 239000013598 vector Substances 0.000 claims description 43
- 238000004364 calculation method Methods 0.000 claims description 12
- 238000010248 power generation Methods 0.000 claims description 8
- 230000007423 decrease Effects 0.000 claims description 2
- 239000012636 effector Substances 0.000 description 98
- 230000006870 function Effects 0.000 description 22
- 238000010586 diagram Methods 0.000 description 9
- 230000033001 locomotion Effects 0.000 description 9
- 238000000034 method Methods 0.000 description 9
- 230000008859 change Effects 0.000 description 8
- 230000000052 comparative effect Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000013459 approach Methods 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000001154 acute effect Effects 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009012 visual motion Effects 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、移動体の制御装置及び制御システムに関する。 The present invention relates to a moving body control device and a control system.
従来、例えばロボット制御技術では、人間がロボットの動作領域に侵入すると危険なため、安全防護柵を設けたり、人間の作業エリアにロボットが侵入できないように規制し、人間とロボットの動作領域を区分してきた。近年、人間とロボットが動作空間を共有して協調作業する取り組みが注目されており、特に、ロボットが各種障害物の存在位置を監視し、自律的に障害物との衝突を回避する手法が開発されつつある（例えば、特許文献１、２参照）。
Conventionally, for example, in robot control technology, it is dangerous for a human to invade the operating area of a robot. I've done it. In recent years, efforts by humans and robots to share the motion space and collaborate have been attracting attention. In particular, a method has been developed in which robots monitor the existence positions of various obstacles and autonomously avoid collisions with obstacles. (See, for example,
例えば、特許文献１記載の技術によれば、全身制御モジュールと、衝突回避モジュールと、混合制御ユニットと、を備えており、算出された衝突危険がより低くなるほど、全身制御モジュールが出力するモーション制御出力信号の重み付け値が高くなり、算出された衝突危険がより低くなるほど、衝突回避モジュールが出力する衝突回避制御出力信号の重み付け値が高くなるようにしている。そして、衝突回避モジュールは、ロボットの異なるセグメント、またセグメントと他の対象物の最も近い２つのポイントを算出し、衝突回避作用を制御するようにしている。
For example, according to the technique described in
特許文献１記載の技術を適用した場合においては、複数の制御モジュールを切替制御するときにロボット（エンドエフェクタ）が不安定な挙動を示す虞がある。すなわち、例えば特許文献１記載の技術において、全身制御モジュールが生成した出力信号のベクトル方向と衝突回避モジュールが生成した出力信号のベクトル方向とが互いに逆方向であった場合、前述の重み付け値に応じて混合制御信号が生成されるものの、この混合制御信号の安定性は保証されていない。このため、ロボットが不安定な挙動を示す虞があり、安定性の高い制御を実行できるようにすることが望まれている。
When the technique described in
この種の課題は、例えば、制御対象の多い多軸多関節型ロボットの他、車両などの移動体の運転アシスト技術や自動運転技術に適用する場合にも同様に適用できるものであり、例えば車両制御分野においては安定性及び安全性の高い制御を実行できるようにすることが望まれている。 This kind of problem can be similarly applied to, for example, multi-axis articulated robots having many controlled objects, as well as driving assist technology and automatic driving technology for moving objects such as vehicles. For example, vehicles. In the field of control, it is desired to be able to perform highly stable and safe control.
本発明の目的は、より安定性の高い制御を実現できるようにした移動体の制御装置及び制御システムを提供することにある。 An object of the present invention is to provide a moving body control device and a control system capable of realizing more stable control.
請求項１記載の発明によれば、混合制御値算出部は、移動体と障害物との間の距離が所定の閾値と等しいときには回避制御値の重み付け値を０とし、到達制御値の重み付け値を０とし、且つ、回避制御値の重み付け値の微分値を０とし、到達制御値の重み付け値の微分値を０とし、混合制御値を算出するようにしている。このため、距離が所定の閾値付近となるときに移動体が不安定な挙動を示すことがなくなり、より安定性の高い制御を実現できる。
According to the invention according to
請求項２記載の発明によれば、混合制御値算出部は、移動体と障害物との間の距離が所定の到達閾値より大きく当該移動体と障害物とが近接する状態のときには回避制御値の重み付け値を０とすると共に到達制御値の重み付け値に切替え、移動体と障害物との間の距離が所定の回避閾値より小さく当該移動体と障害物とが遠ざかる状態のときには到達制御値の重み付け値を０とすると共に回避制御値の重み付け値に切替えている。このため、より信頼性を向上した回避制御を実行できると共に、目標に到達させるために必要な時間の増大を極力抑制でき、より安定性の高い制御を実現できる。
According to the invention according to
請求項６記載の発明によれば、計時部は、障害物が一定領域内に留まる時間を計時し、動力発生制御値出力部は、計時部により一定領域内に留まる時間を計時することで障害物が静止障害物と判定されたときには、障害物の位置と移動体の位置に基づいて当該移動体に動力を発生させる制御値を出力することで移動体と静止障害物との相対位置関係の硬直状態を解消する。これにより、障害物との距離に応じて硬直状態となっても当該硬直状態を解消できる。 According to the invention of claim 6, the timekeeping unit measures the time for the obstacle to stay in the fixed area, and the power generation control value output unit measures the time for the obstacle to stay in the fixed area by the timekeeping unit. When an object is determined to be a stationary obstacle, the relative positional relationship between the moving object and the stationary obstacle is determined by outputting a control value for generating power to the moving object based on the position of the obstacle and the position of the moving object. Eliminate the rigid state. As a result, even if the rigid state is established according to the distance to the obstacle, the rigid state can be eliminated.
以下、移動体の制御装置及び制御システムの幾つかの実施形態を説明する。以下の実施形態中では、各実施形態間で同一機能または類似機能を備えた部分に同一符号又は類似符号（例えば十の位及び一の位を同一値）を付して説明を行い、後述実施形態においては同一機能または類似機能を備えた部分の説明を必要に応じて省略する。 Hereinafter, some embodiments of the control device and the control system of the moving body will be described. In the following embodiments, the same code or a similar code (for example, the same value for the tens digit and the ones digit) will be added to the portions having the same function or the similar function between the embodiments, and the description will be described later. In the form, the description of the part having the same function or the similar function will be omitted as necessary.
（第１実施形態）
図１から図７は第１実施形態の説明図を示す。図１は移動体の制御システムの全体構成を示し、一例としてロボット１のエンドエフェクタ２の制御システム３を示している。また図２は制御システムの電気的構成を機能的なブロック図により表している。
(First Embodiment)
1 to 7 show explanatory views of the first embodiment. FIG. 1 shows the overall configuration of a moving body control system, and shows the
図１に示すロボット１は、例えば多関節型のロボットアームを備え、例えばロータリージョイント４などの少なくとも一つ以上のジョイント４～７を介して互いに連結されたセグメントアーム４、８、９を用いて構成される。このロボット１のセグメントアーム４の先端にはロボットハンドなどのエンドエフェクタ２が移動体として取付けられている。なお図１には、ロボット１について２次元的な側面図により簡略的に図示しているが、３次元方向の何れの位置にもエンドエフェクタ２を移動制御可能になっている。ロボット１には制御装置１０及びセンサ１１が電気的に接続されている。
The
図２に示すように、この制御システム３は、制御装置１０、センサ１１、及びロボットジョイントコントローラ１２を備える。制御装置１０はプログラムが記憶される記憶部１３を備える。図３に示すように、制御システム３は記憶部１３に記憶されたプログラムに基づいて、障害物Ｏを避けながら目標Ｔの位置までエンドエフェクタ２を移動制御する。ここでは、エンドエフェクタ２と障害物Ｏとの間の距離をＤと定義している。
As shown in FIG. 2, the
センサ１１は、図１には簡略的に示しているが例えばロボット１のエンドエフェクタ２と共にその周辺の障害物Ｏ及び目標Ｔを含む画像を撮像する撮像装置を備えている。このセンサ１１は、例えば畳み込みニューラルネットワークを用いた画像認識技術を適用し、撮像装置により撮像された画像データから所定の形状やパターンを認識し、当該画像データに含まれる目標Ｔや障害物Ｏを区別して画像認識処理を行い、これらの位置情報（すなわち目標Ｔの位置情報ＬT、障害物Ｏの位置情報Ｌｏ）を制御装置１０に出力する。
Although simply shown in FIG. 1, the
制御装置１０は、例えば、ＲＯＭ、ＲＡＭ、不揮発性メモリ等の記憶部１３を備えたマイクロコンピュータを主として構成され、センサ１１から取得される位置情報を用いて、視覚運動の連携に基づくニューラルネットワークを用いて目標Ｔの位置を学習し、各ジョイント４～７の角度及び角速度を制御し、これによりエンドエフェクタ２を目標Ｔまで移動制御する。
The
この制御装置１０は、このような移動制御を実現するため、二段階のコントローラとして、到達モジュール１４、回避モジュール１５、及び、混合モジュール１６を備える。到達モジュール１４は、各ジョイント４～７の角度及び角速度並びにエンドエフェクタ２の３次元位置の情報をロボットジョイントコントローラ１２から入力し、このエンドエフェクタ２の位置とセンサ１１により出力される目標Ｔの位置ＬTとの間の距離Ｄに基づく学習済みのニューラルネットワークに応じて到達制御値Ｃｒを算出し、この到達制御値Ｃｒを混合モジュール１６に出力する到達制御値出力部としての機能を備える。
In order to realize such movement control, the
回避モジュール１５は、各ジョイント４～７の角度及び角速度情報並びにエンドエフェクタ２の３次元位置の情報をロボットジョイントコントローラ１２から入力し、このエンドエフェクタ２の位置とセンサ１１により出力される障害物Ｏの位置情報Ｌｏとの間の距離Ｄに基づく学習済みのニューラルネットワークに応じて回避制御値Ｃａを算出し、この回避制御値Ｃａを混合モジュール１６に出力する回避制御値出力部としての機能を備える。
The
混合モジュール１６は、センサ１１により出力される位置情報ＬT，Ｌｏ、到達モジュール１４により出力される到達制御値Ｃｒ、回避モジュール１５により出力される回避制御値Ｃａ、及び、エンドエフェクタ２の位置と障害物Ｏの位置Ｌｏとの間の距離Ｄに応じて定められる重み付け値、に応じて混合制御値Ｃｍを算出する混合制御値算出部としての機能を備える。
The
このとき例えば混合モジュール１６は、センサ１１により取得される障害物Ｏの位置情報Ｌｏとエンドエフェクタ２の位置との間の距離Ｄに応じて定められる重み付け値Ｒｅ，Ａｖを到達制御値Ｃｒと回避制御値Ｃａに所定の演算処理（例えば、何れか一方の制御値Ｃｒ又はＣａを０とし他方の制御値Ｃａ又はＣｒに重み付け演算処理）することで混合制御値Ｃｍを算出し、この混合制御値Ｃｍをロボットジョイントコントローラ１２に出力する。ロボットジョイントコントローラ１２は、この混合制御値Ｃｍに応じてジョイント４～７の角度及び角速度を制御し、障害物Ｏを避けつつ目標Ｔまでエンドエフェクタ２を移動制御する。
At this time, for example, the
上記基本的構成の特徴的な作用について図３～図６をも参照しながら説明する。図３はエンドエフェクタ２の位置、その移動目標Ｔの位置ＬT、及び、障害物Ｏの位置Ｌｏを概略的に示す平面図である。
The characteristic operation of the above basic configuration will be described with reference to FIGS. 3 to 6. FIG. 3 is a plan view schematically showing the position of the
ここでは、センサ１１を構成する撮像装置が、エンドエフェクタ２、目標Ｔ、障害物Ｏの上方から撮像するように設置され、当該センサ１１が上方からこれらのエンドエフェクタ２、目標Ｔ、障害物Ｏの位置Ｌｏを画像認識技術により検出可能にする形態を示し、説明の簡略化のため、制御装置１０がエンドエフェクタ２を二次元的に目標Ｔの位置ＬTに移動制御する形態を示す。図３に示すように、エンドエフェクタ２と目標Ｔの位置ＬTとの間の直線上に障害物Ｏが存在する場合を考慮する。
Here, the image pickup apparatus constituting the
図４はエンドエフェクタ２と障害物Ｏとの間の距離Ｄに応じた重み付け値Ｒｅ，Ａｖの関係性について示している。記憶部１３には、これらの重み付け値Ｒｅ，Ａｖと距離Ｄとの間の関係性が例えばテーブル又は数式等を用いて記憶されており、混合モジュール１６は、この記憶部１３に記憶された重み付け値Ｒｅ，Ａｖの関係性に応じて混合制御値Ｃｍを算出する。ここでは、エンドエフェクタ２の障害物Ｏの位置Ｌｏからの安全確保可能距離として閾値Ｄ０を定義している。
FIG. 4 shows the relationship between the weighted values Re and Av according to the distance D between the
図４に示すように、回避制御値Ｃａの重み付け値Ａｖは、エンドエフェクタ２の障害物Ｏとの間の距離が低閾値Ｄｄ（但しＤｄ＜Ｄ０）より小さいときには１とされており、ある低閾値Ｄｄを境界として距離Ｄが大きくなると単純減少し、距離Ｄが閾値Ｄ０のときに０となる。そして距離Ｄが閾値Ｄ０を含み当該Ｄ０以上のときには距離Ｄが大きくなっても重み付け値Ａｖは０になるように設定されている。
As shown in FIG. 4, the weighted value Av of the avoidance control value Ca is set to 1 when the distance between the
この回避制御値Ｃａの重み付け値Ａｖは、少なくとも距離Ｄ０以下で且つ低閾値Ｄｄを超える距離Ｄの領域において当該距離Ｄにより微分可能な関数により表現可能となっている。この回避制御値Ｃａの重み付け値Ａｖを関数により表現した場合、距離Ｄ０における回避制御値Ｃａの重み付け値Ａｖ（Ｄ０）の微分値が０となるようにパラメータ調整されている。すなわち回避制御値Ｃａの重み付け値Ａｖは距離Ｄ０付近において緩やかに変化することになる。 The weighted value Av of the avoidance control value Ca can be expressed by a function that is differentiable by the distance D in the region of the distance D that is at least the distance D0 or less and exceeds the low threshold value Dd. When the weighted value Av of the avoidance control value Ca is expressed by a function, the parameters are adjusted so that the differential value of the weighted value Av (D0) of the avoidance control value Ca at the distance D0 becomes 0. That is, the weighted value Av of the avoidance control value Ca gradually changes near the distance D0.
到達制御値Ｃｒの重み付け値Ｒｅは、エンドエフェクタ２の障害物Ｏとの距離Ｄが０に近く閾値Ｄ０より小さいときには０に設定されており、閾値Ｄ０を境界として距離Ｄが大きくなると単純増加し、ある高閾値Ｄｕ（但しＤｕ＞Ｄ０）のときに１となり、当該高閾値Ｄｕを境界として距離Ｄが大きくなると１に設定される。
The weighted value Re of the arrival control value Cr is set to 0 when the distance D of the
また、到達制御値Ｃｒの重み付け値Ｒｅは、少なくとも距離Ｄ０以上で且つ高閾値Ｄｕを下回る距離Ｄの領域において距離Ｄにより微分可能な関数（例えば距離Ｄの二乗に応じて変化する関数）により表現可能となっており、この関数は、距離Ｄ０における到達制御値Ｃｒの重み付け値Ｒｅ（Ｄ０）の微分値が０となるように設定されている。すなわち到達制御値Ｃｒの重み付け値Ｒｅは距離Ｄ０付近において緩やかに変化することになる。 Further, the weighted value Re of the arrival control value Cr is expressed by a function that is differentiable by the distance D (for example, a function that changes according to the square of the distance D) in the region of the distance D that is at least the distance D0 or more and is lower than the high threshold Du. This function is possible, and this function is set so that the differential value of the weighted value Re (D0) of the arrival control value Cr at the distance D0 becomes 0. That is, the weighted value Re of the arrival control value Cr changes gently in the vicinity of the distance D0.
すなわち、エンドエフェクタ２と障害物Ｏの距離Ｄが閾値Ｄ０に一致するとき、回避制御値Ｃａの重み付け値Ａｖと到達制御値Ｃｒの重み付け値Ｒｅはそれぞれ０に設定され、回避制御値Ｃａの重み付け値Ａｖと到達制御値Ｃｒの重み付け値Ｒｅの距離Ｄによる微分値もそれぞれ０になっている。
That is, when the distance D between the
図５には、これらの重み付け値Ａｖ，Ｒｅを用いて混合モジュール１６が生成した混合制御値Ｃｍの一例を示す。この図５においては、縦軸正方向が回避制御方向を示しており、縦軸負方向が到達制御方向を示しており、制御装置１０が混合制御値Ｃｍを０にすることでエンドエフェクタ２を停止制御することを示す。図５に示すように、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０よりも近いときには混合制御値Ｃｍは回避制御方向（すなわち正値）となり、距離Ｄが閾値Ｄ０よりも遠いときには混合制御値Ｃｍは到達制御方向（すなわち負値）となる。
FIG. 5 shows an example of the mixing control value Cm generated by the mixing
前述したように、到達制御値Ｃｒ及び回避制御値Ｃａの重み付け値Ａｖ，Ｒｅは、閾値Ｄ０付近において前述した条件を満たすように設定されているため、この閾値Ｄ０付近の例えば下限値Ｄｄ１から上限値Ｄｕ１の範囲における混合制御値Ｃｍを緩やかに変化させることができる。このため、到達制御方向と回避制御方向の境界点、すなわち距離Ｄの閾値Ｄ０において緩やかに制御方向を切替えることができ、エンドエフェクタ２の挙動をより安定的に制御できる。
As described above, since the weighted values Av and Re of the arrival control value Cr and the avoidance control value Ca are set so as to satisfy the above-mentioned conditions near the threshold value D0, for example, the upper limit is from the lower limit value Dd1 near the threshold value D0. The mixing control value Cm in the range of the value Du1 can be gradually changed. Therefore, the control direction can be gently switched at the boundary point between the arrival control direction and the avoidance control direction, that is, the threshold value D0 of the distance D, and the behavior of the
＜比較例の説明＞
図６及び図７は発明者らが考慮した比較例の説明図を示している。この比較例では、閾値Ｄ０付近で重み付け値Ａｖ，Ｒｅを前述のように設けることなく、距離Ｄ０を境界として回避モジュール１５の回避制御値Ｃａと到達モジュール１４の到達制御値Ｃｒとを単純に切り替える例を示している。図６に示すように、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０よりも近いときに混合制御値Ｃｍは回避制御方向の値となり、距離Ｄが閾値Ｄ０よりも遠いときに混合制御値Ｃｍは到達制御方向の値となるが、この境界の閾値Ｄ０付近では混合制御値Ｃｍが急激に変化する。図７に、そのときのエンドエフェクタ２の挙動イメージ図を示しているが、エンドエフェクタ２による障害物Ｏとの距離Ｄに応じて障害物Ｏに近接したり回避したりする挙動を示す。このため、エンドエフェクタ２が不安定な挙動を示すことになる。
<Explanation of comparative example>
6 and 7 show explanatory diagrams of comparative examples considered by the inventors. In this comparative example, the avoidance control value Ca of the
＜概念的なまとめ＞
要するに、本実施形態によれば、距離Ｄが所定の閾値Ｄ０に等しいときには回避制御値Ｃａの重み付け値Ａｖを０とし、到達制御値Ｃｒの重み付け値Ｒｅを０とし、且つ、回避制御値Ｃａの重み付け値Ａｖの微分値を０とし、到達制御値Ｃｒの重み付け値Ｒｅの微分値を０としている。このような条件を用いることで、到達制御と回避制御とを繰り返す制御動作を極力なくすことができ、エンドエフェクタ２の挙動をより安定的に制御できる。これにより安定性の高い制御を実現できる。
<Conceptual summary>
In short, according to the present embodiment, when the distance D is equal to the predetermined threshold value D0, the weighting value Av of the avoidance control value Ca is set to 0, the weighting value Re of the arrival control value Cr is set to 0 , and the avoidance control value is set to 0. The differential value of the weighted value Av of Ca is 0, and the differential value of the weighted value Re of the arrival control value Cr is 0 . By using such a condition, the control operation of repeating the arrival control and the avoidance control can be eliminated as much as possible, and the behavior of the
（第２実施形態）
図８及び図９は第２実施形態の追加説明図を示している。図８はエンドエフェクタ２が障害物Ｏに近接した位置から遠ざかるときの回避制御値Ｃａの重み付け値Ａｖの変化を示しており、図９はエンドエフェクタ２が遠方から障害物Ｏに近接するときの到達制御値Ｃｒの重み付け値Ｒｅの変化を示している。
(Second Embodiment)
8 and 9 show additional explanatory views of the second embodiment. FIG. 8 shows the change in the weighting value Av of the avoidance control value Ca when the
センサ１１は撮像された画像をリアルタイム処理することで、時間経過に伴い変化する障害物Ｏの位置Ｌｏを繰り返し検出でき、また目標Ｔの位置ＬTも繰り返し検出できる。このため混合モジュール１６は、エンドエフェクタ２と障害物Ｏとの間の距離Ｄを算出したときに、これらの距離Ｄが時間変化に伴い大きくなるときにはエンドエフェクタ２が障害物Ｏから遠ざかっていると判定でき、距離Ｄが時間変化に伴い小さくなるときにはエンドエフェクタ２が障害物Ｏに近接していると判定できる。図８に示すように、エンドエフェクタ２が障害物Ｏから遠ざかると判定されたときには、混合モジュール１６は、前述実施形態で示した閾値Ｄ０に代えて回避閾値Ｄｈｕを回避制御値Ｃａの重み付け値Ａｖの境界として用いる。
By processing the captured image in real time, the
すなわち混合モジュール１６は、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが回避閾値Ｄｈｕとなるときに、回避制御値Ｃａの重み付け値Ａｖを０とし、且つ、この閾値Ｄｈｕにおける重み付け値Ａｖの微分値も０になる関数を用いて制御する。回避閾値Ｄｈｕを超えても回避制御値Ｃａの重み付け値Ａｖを０にする関数を用いると良い。また前述実施形態で説明したように許容範囲を設けることが望ましい。なお明確に図示はしていないが、混合モジュール１６は、距離Ｄが回避閾値Ｄｈｕを下回るときには到達制御値Ｃｒの重み付け値Ｒｅを０に設定し、少なくとも回避閾値Ｄｈｕ付近において到達制御値Ｃｒの重み付け値Ｒｅの微分値も０とし、回避閾値Ｄｈｕを超えると重み付け値Ｒｅを緩やかに上昇変化させる関数を適用する。
That is, when the distance D between the
また図９に示すように、エンドエフェクタ２が遠方から障害物Ｏに近接する状態のときには、閾値Ｄ０に代えて到達閾値Ｄｈｄを到達制御値Ｃｒの重み付け値の境界として用いる。すなわち混合モジュール１６は、距離Ｄが到達閾値Ｄｈｄとなるときに到達制御値Ｃｒの重み付け値Ｒｅを０とし、且つ、この到達閾値Ｄｈｄにおける重み付け値Ｒｅの微分値もまた０になるような関数を用いて制御する。また前述実施形態で説明したように許容範囲を設けることが望ましい。なお明確に図示はしていないが、混合モジュール１６は、距離Ｄが到達閾値Ｄｈｄ以上であるときには回避制御値Ｃａの重み付け値Ａｖを０に設定し、少なくとも到達閾値Ｄｈｄ付近において回避制御値Ｃａの重み付け値Ａｖの微分値も０とし、到達閾値Ｄｈｄを下回ると重み付け値Ａｖを緩やかに上昇変化させる関数を適用する。
Further, as shown in FIG. 9, when the
また回避閾値Ｄｈｕは到達閾値Ｄｈｄよりも高く設定されていることが望ましい。この場合、エンドエフェクタ２が障害物Ｏに必要以上に近接しているときには、距離Ｄが回避閾値Ｄｈｕに達するまで回避制御値Ｃａを用いて優先的に切替制御することになるため、障害物Ｏの極力遠方まで回避制御を優先させることができる。これにより、信頼性良く障害物Ｏへの衝突を回避できる。
Further, it is desirable that the avoidance threshold Dhu is set higher than the arrival threshold Dhd. In this case, when the
逆に、エンドエフェクタ２が遠方から障害物Ｏ及び目標Ｔに近接するときには、障害物Ｏとの距離Ｄが到達閾値Ｄｈｄに達するまで到達制御値Ｃｒを用いて優先的に切替制御することになるため、極力障害物Ｏの近くまで回避制御を機能させることなく目標Ｔの近くまで到達制御できる。例えば、エンドエフェクタ２と障害物Ｏと目標Ｔとの位置関係が、図３に示す関係となるときには、目標Ｔの位置により近接するまで到達制御を継続することができ、エンドエフェクタ２を目標Ｔの位置に到達させるために必要な時間の増大を極力抑制できる。
On the contrary, when the
また、回避閾値Ｄｈｕ＜到達閾値Ｄｈｄとしても良い。回避閾値Ｄｈｕ＜距離Ｄ＜到達閾値Ｄｈｄを満たすときには、到達制御値Ｃｒ及び回避制御値Ｃａの重み付け値Ｒｅ，Ａｖを共に０とすることが望ましく、この条件を満たす距離Ｄの場合には、エンドエフェクタ２は原則的に停止することになり、より安全性能を優先したい場合に有効である。
Further, the avoidance threshold Dhu <reaching threshold Dhd may be set. When the avoidance threshold Dhu <distance D <reaching threshold Dhd is satisfied, it is desirable that the weighted values Re and Av of the arrival control value Cr and the avoidance control value Ca are both set to 0. The
＜本実施形態の概念的なまとめ＞
要するに、本実施形態によれば、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが所定の到達閾値Ｄｈｄより大きくエンドエフェクタ２と障害物Ｏとが近接する状態のときには回避制御値Ｃａの重み付け値Ａｖを０とし到達制御値Ｃｒの重み付け値Ｒｅに切替え、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが所定の回避閾値Ｄｈｕより小さくエンドエフェクタ２と障害物Ｏとが遠ざかる状態のときには到達制御値Ｃｒの重み付け値Ｒｅを０とし回避制御値Ｃａの重み付け値Ａｖに切替えている。これにより、より信頼性を向上した回避制御を実行できると共に、目標Ｔに到達させるために必要な時間の増大を極力抑制でき、より安定性の高い制御を実現できる。
<Conceptual summary of this embodiment>
In short, according to the present embodiment, the avoidance control value Ca is weighted when the distance D between the
（第３実施形態）
図１０は第３実施形態の追加説明図を示す。第３実施形態においては、混合モジュール１６が、障害物Ｏの位置Ｌｏ及び速度、エンドエフェクタ２の位置及び速度、を判定条件として用いて到達制御値Ｃｒ及び回避制御値Ｃａに重み付けして混合制御値Ｃｍを算出する形態を示す。特に、エンドエフェクタ２が障害物Ｏから遠ざかる方向に移動しているときには、たとえ距離Ｄが閾値Ｄ０未満であっても到達モジュール１４の到達制御値Ｃｒを用いて制御可能にした形態を示す。
(Third Embodiment)
FIG. 10 shows an additional explanatory diagram of the third embodiment. In the third embodiment, the mixing
以下では、説明の便宜上、ある基準位置からのエンドエフェクタ２の位置ベクトルと、エンドエフェクタ２の速度ベクトルをそれぞれ、「→Ｄ０」、「→Ｖ０」、と表記し、基準位置からの障害物Ｏの位置ベクトルと、その障害物Ｏの速度ベクトルを、それぞれ、「→Ｄ１」、「→Ｖ１」、と表記する。
In the following, for convenience of explanation, the position vector of the
前述実施形態で示したエンドエフェクタ２と障害物Ｏとの間の距離Ｄを言い換えると、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜として表すことができ、この相対距離ベクトル「→Ｄ」は「→Ｄ０」－「→Ｄ１」により定義できる。また、エンドエフェクタ２と障害物Ｏがそれぞれ速度べクトル「→Ｖ０」，「→Ｖ１」で移動しているため、これらの相対速度ベクトルを「→Ｖ」＝「→Ｖ０」－「→Ｖ１」と表すことができる。なお、前述実施形態の機能によれば、混合モジュール１６は、相対距離ベクトルのスカラー値｜→Ｄ｜が閾値Ｄ０を境界とした条件を用いて到達制御値Ｃｒ及び回避制御値Ｃａに重み付けして混合制御値Ｃｍを算出することになる。
In other words, the distance D between the
本実施形態に係る機能説明を理解しやすくするため、前述実施形態で説明した回避制御値Ｃａ及び到達制御値Ｃｒの重み付け値Ａｖ，Ｒｅを単純化して０又は１とし、混合モジュール１６は、当該回避制御値Ｃａまたは到達制御値Ｃｒを選択切替して混合制御値Ｃｍを算出する形態を説明する。すなわちここでは、混合モジュール１６は、何れかのモジュール１４，１５の回避制御値Ｃｒ又は到達制御値Ｃｒの出力を混合制御値Ｃｍとする例を示す。
In order to make it easier to understand the functional explanation according to the present embodiment, the weighted values Av and Re of the avoidance control value Ca and the arrival control value Cr described in the above-described embodiment are simplified to 0 or 1, and the
図１０は条件に応じた混合制御値Ｃｍの設定方法をフローチャートで示している。この図１０に示すように、混合モジュール１６は、相対距離ベクトルのスカラー値｜→Ｄ｜を算出し、Ｓ１において｜→Ｄ｜≧Ｄ０であると判定したときには、混合モジュール１６は、Ｓ１でＹＥＳと判定し、回避制御値Ｃａの重み付け値Ａｖを０とし到達制御値Ｃｒの重み付け値Ｒｅを１とし、これによりＳ２において到達モジュール１４の到達制御値Ｃｒに切替えて混合制御値Ｃｍとして出力する。
FIG. 10 is a flowchart showing a method of setting the mixing control value Cm according to the conditions. As shown in FIG. 10, the mixing
また本実施形態では、混合モジュール１６は相対距離ベクトルのスカラー値｜→Ｄ｜＜Ｄ０のときにＳ１でＮＯと判定し、Ｓ３において相対速度べクトル「→Ｖ」を判定条件として用いてＳ４又はＳ５において混合制御値Ｃｍを算出する。
Further, in the present embodiment, the mixing
ここで、Ｓ３において相対速度ベクトル「→Ｖ」と相対距離ベクトル「→Ｄ」の内積値「→Ｖ」・「→Ｄ」≧０の条件を満たすときには、Ｓ３でＹＥＳと判定し混合制御値Ｃｍを到達制御値Ｃｒに切替えて出力する。 Here, when the condition of the internal product value “→ V” / “→ D” ≧ 0 of the relative velocity vector “→ V” and the relative distance vector “→ D” is satisfied in S3, YES is determined in S3 and the mixing control value Cm. Is switched to the arrival control value Cr and output.
これらのベクトルの内積値「→Ｖ」・「→Ｄ」≧０のときには、相対距離ベクトル「→Ｄ」と相対速度ベクトル「→Ｖ」の角度が鋭角又は垂直となるため、エンドエフェクタ２が障害物Ｏから遠ざかる方向に移動していることを示す。このため、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜がたとえ閾値Ｄ０未満となっても、混合制御値Ｃｍとして到達制御値Ｃｒを用いて制御しても障害物Ｏに衝突することはない。
When the internal product values of these vectors "→ V" and "→ D" ≥ 0, the angle between the relative distance vector "→ D" and the relative velocity vector "→ V" becomes an acute angle or vertical, so that the
逆に、混合モジュール１６は、この内積値「→Ｖ」・「→Ｄ」が０未満のときには、Ｓ３でＮＯと判定しＳ５において混合制御値Ｃｍを回避制御値Ｃａに切替えて出力する。この内積値「→Ｖ」・「→Ｄ」が０未満のときには、相対距離ベクトル「→Ｄ」と相対速度ベクトル「→Ｖ」の角度が鈍角となるため、エンドエフェクタ２と障害物Ｏとが相対的に近接する方向に移動していることを示す。
On the contrary, when the inner product values “→ V” and “→ D” are less than 0, the mixing
このため、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜が閾値Ｄ０未満となりＳ１でＮＯと判定され、Ｓ３でＮＯと判定されたときには、混合モジュール１６が回避制御値Ｃａを用いて混合制御値Ｃｍを生成することが望ましい。これにより、エンドエフェクタ２が障害物Ｏに衝突することを回避できる。
Therefore, when the scalar value | → D | of the relative distance vector "→ D" is less than the threshold value D0 and is determined to be NO in S1 and NO in S3, the mixing
＜変形例＞
ここでは説明の簡単化のため、重み付け値Ａｖ、Ｒｅを０または１とした形態を示したが、これに限定されるものではなく、第１又は第２実施形態に示した条件を用いて重み付け値Ａｖ、Ｒｅを０を超え１までの値とし、これらの重み付け値Ａｖ、Ｒｅを用いて混合制御値Ｃｍを算出するようにしても良い。例えば混合モジュール１６は、回避制御値Ｃａに０を超え１までの重み付け値Ａｖを乗算して混合制御値Ｃｍとして出力したり、到達制御値Ｃｒに重み付け値Ｒｅを乗算して混合制御値Ｃｍとして出力したりするように構成できる。これによりエンドエフェクタ２の挙動を安定化できる。
<Modification example>
Here, for the sake of simplification of the explanation, a form in which the weighting values Av and Re are set to 0 or 1 is shown, but the weighting is not limited to this, and weighting is performed using the conditions shown in the first or second embodiment. The values Av and Re may be set to a value exceeding 0 and up to 1, and the mixed control value Cm may be calculated using these weighted values Av and Re. For example, the mixing
＜本実施形態の概念的なまとめ＞
本実施形態によれば、混合モジュール１６は、エンドエフェクタ２と障害物Ｏの相対速度ベクトル「→Ｖ」を用いて制御しているので、たとえ障害物Ｏとエンドエフェクタ２の相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜が所定の閾値Ｄ０未満であったとしても、エンドエフェクタ２が障害物Ｏに接近していない限り到達制御値Ｃｒを用いて制御することになる。このため回避制御値Ｃａを用いて制御することはなくなり、エンドエフェクタ２をより目標Ｔの位置に近接するまで到達制御させることができ、目標Ｔの位置に到達させるために必要な時間の増大を極力抑制できる。これによりエンドエフェクタ２を目標Ｔの位置まで効率的に移動制御できる。
<Conceptual summary of this embodiment>
According to the present embodiment, since the mixing
（第４実施形態）
図１１及び図１２は第４実施形態の追加説明図を示している。例えば、第１から第３の何れかの実施形態の制御方式を考慮した場合、エンドエフェクタ２又は障害物Ｏの何れか一方が移動すれば、これらの間の距離Ｄも変化する。このため、エンドエフェクタ２と障害物Ｏとの距離Ｄが変化することから、重み付け値Ａｖ，Ｒｅ又は各制御値Ｃｒ，Ｃａも変化し、障害物Ｏとの距離Ｄ及び目標Ｔとの距離に応じてエンドエフェクタ２を移動制御できるようになる。
(Fourth Embodiment)
11 and 12 show additional explanatory views of the fourth embodiment. For example, when the control method of any one of the first to third embodiments is taken into consideration, if either the
しかし、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０に一致すると、障害物Ｏが一定の領域内に留まると共に、エンドエフェクタ２も一定の領域内に留まる。このとき、到達モジュール１４の到達制御値Ｃｒ、回避モジュール１５の回避制御値Ｃａは共に０となるため、混合制御値Ｃｍも０となり、目標到達制御は原理的に実行されなくなる。また、たとえ混合制御値Ｃｍが０にならなくても、混合制御値Ｃｍが０に近く小さくなる場合には、エンドエフェクタ２の動作も遅くなり、エンドエフェクタ２の挙動も不安定になりやすい。
However, when the distance D between the
このようなとき、効率的に障害物Ｏを回避するため、本実施形態の制御システム１０３においては、図１１に示すように計時モジュール１７を混合モジュール１１６に接続するように設けている。この計時モジュール１７は、障害物Ｏが一定領域内に留まる時間を計測する計時部としての機能を備える。混合モジュール１１６は、到達モジュール１４の到達制御値Ｃｒ、回避モジュール１５の回避制御値Ｃａが共に０又は０を含む所定範囲となったときに計時モジュール１７にトリガを出力し、計時モジュール１７がこのトリガを受付けることで経過時間を計時開始する。
In such a case, in order to efficiently avoid the obstacle O, the
混合モジュール１１６は、計時モジュール１７により経過時間を検出するが、到達制御値Ｃｒ及び回避制御値Ｃａが共に０を含む許容範囲内にある経過時間を検出することで障害物Ｏの運動状態を判定する。混合モジュール１１６は、この条件を満たす経過時間がある閾値を超えた場合に障害物Ｏを静止障害物と判定する。
The
混合モジュール１１６が障害物Ｏを静止障害物と判定したときには、エンドエフェクタ２の中心位置と障害物Ｏの中心位置とを通過する直線に対し、例えば水平面内で垂直な方向に動力を与えるような混合制御値Ｃｍを生成し、ロボットジョイントコントローラ１２に出力する。垂直方向に限らず直線に交差する方向に動力を与えるような混合制御値Ｃｍを生成しロボットジョイントコントローラ１２に出力するようにしても良い。
When the
このとき混合モジュール１１６はエンドエフェクタ２に動力を発生させるための制御値を出力する動力発生制御値出力部として機能することになる。このとき生成される混合制御値Ｃｍは、目標到達制御が実行停止したときの混合制御値Ｃｍによる動力に比較して大幅に大きな動力となるように生成される。
At this time, the
するとエンドエフェクタ２は、この動力に応じた混合制御値Ｃｍに基づいて障害物Ｏとを結ぶ直線に対し交差方向に動作し始める。図１２にはエンドエフェクタ２の初動方向の一例を示している。これにより、エンドエフェクタ２と障害物Ｏとの間の相対位置関係の硬直状態を解消できる。しかも、エンドエフェクタ２が、当該エンドエフェクタ２と障害物Ｏとの間を結ぶ直線に対し垂直方向に動作することで障害物Ｏを効率的に回避できる。なお、エンドエフェクタ２の初動後の動作については、混合モジュール１６が到達制御値Ｃｒ及び回避制御値Ｃａに基づいて混合制御値Ｃｍを算出し、この混合制御値Ｃｍをロボットジョイントコントローラ１２に出力することになる。これにより前述実施形態と同様の動作を実行できる。
Then, the
＜本実施形態の概念的なまとめ＞
本実施形態によれば、計時モジュール１７により障害物Ｏが一定領域内に留まる時間を計時し、混合モジュール１１６が障害物Ｏの運動状態を判定し静止障害物であると判定したときには、混合モジュール１１６は障害物Ｏの位置とエンドエフェクタ２の位置に基づいてエンドエフェクタ２に動力を発生させるように混合制御値Ｃｍを出力する。これによりエンドエフェクタ２と障害物Ｏとの相対位置関係の硬直状態を解消できる。
<Conceptual summary of this embodiment>
According to the present embodiment, when the time of the obstacle O staying in a certain area is timed by the
（第５実施形態）
図１３は第５実施形態の追加説明図を示している。第４実施形態ではエンドエフェクタ２の位置と障害物Ｏの位置との関係に基づいてエンドエフェクタ２に動力を発生させる形態を説明したが、これに限定されるものではなく、さらに目標Ｔの位置に応じてエンドエフェクタ２に印加する動力の方向を制御するようにしても良い。
(Fifth Embodiment)
FIG. 13 shows an additional explanatory diagram of the fifth embodiment. In the fourth embodiment, a mode in which power is generated in the
目標Ｔの位置、エンドエフェクタ２の位置、障害物Ｏの位置、の各々のベクトルを「→Ｄ０」、「→Ｄ１」、「→Ｄ２」とし、エンドエフェクタ２に印加する力のベクトルを「→Ｆ」とする。このとき、印加する力のベクトル「→Ｆ」の方向は、下記の２つの式を満たすように定義することが望ましい。
The vectors of the target T position, the
（「→Ｄ２」－「→Ｄ１」）・「→Ｆ」＝０ …（１）
（「→Ｄ０」－「→Ｄ１」）・「→Ｆ」≧０ …（２）
すなわち（１）式は、エンドエフェクタ２から障害物Ｏに向かうベクトル（「→Ｄ２」－「→Ｄ１」）との間で内積値が０を満たす、すなわち、このベクトル（「→Ｄ２」－「→Ｄ１」）の方向と力ベクトル「→Ｆ」の方向との角度が垂直となる関係を満たす力ベクトル「→Ｆ」を定義している。
("→ D2"-"→ D1") ・ "→ F" = 0 ... (1)
(“→ D0”-“→ D1”) ・ “→ F” ≧ 0… (2)
That is, in the equation (1), the internal product value satisfies 0 between the vector ("→ D2"-"→ D1") from the
また（２）式は、エンドエフェクタ２の位置から目標Ｔの位置に向かうベクトル（「→Ｄ０」－「→Ｄ１」）との間で内積値が０以上となる、すなわち、このベクトル（「→Ｄ０」－「→Ｄ１」）の方向と力ベクトル「→Ｆ」の方向との角度が鋭角となるように力ベクトル「→Ｆ」を定義している。このとき、力ベクトル「→Ｆ」は、図１３に示すように目標Ｔの位置に少しでも近接する方向となり、エンドエフェクタ２は目標Ｔの位置に少しでも近接するように移動することになる。このため、障害物Ｏを回避しながら目標Ｔの位置に少しでも近づくことができるようになり効率的に移動制御できる。
Further, in the equation (2), the internal product value becomes 0 or more between the vector (“→ D0”-“→ D1”) from the position of the
本実施形態によれば、障害物Ｏが静止障害物と判定されたときには、障害物Ｏの位置とエンドエフェクタ２の位置と目標Ｔの位置とに基づいてエンドエフェクタ２に動力を発生させる制御値を出力するため、エンドエフェクタ２と障害物Ｏとの相対位置関係の硬直状態を解消できる。特に（１）（２）式を満たすように制御しているため、障害物Ｏを回避しながら目標Ｔの位置に効率的に移動制御できる。
According to the present embodiment, when the obstacle O is determined to be a stationary obstacle, a control value for generating power to the
（他の実施形態）
本発明は、前述実施形態に限定されるものではなく、例えば、以下に示す変形又は拡張が可能である。
(Other embodiments)
The present invention is not limited to the above-described embodiment, and for example, the following modifications or extensions are possible.
前述実施形態では説明の簡略化のため、第１、第２実施形態では１次元、第３から第５の実施形態では２次元の制御を考慮した形態を示したが、全ての実施形態において２次元方向及び３次元方向の制御に適用できる。 In the above-described embodiment, for the sake of simplification of the description, one-dimensional control is shown in the first and second embodiments, and two-dimensional control is taken into consideration in the third to fifth embodiments. It can be applied to control in dimensional direction and three-dimensional direction.
「目標位置センサ」、「障害物位置センサ」、について、撮像装置を搭載したセンサ１１により障害物Ｏの位置や目標Ｔの位置を検出するように構成した形態を示したが、これに限定されるものではなく、例えば位置検出用の超音波センサ等を適用しても良いし、各種のセンサやその制御装置を一体に設けても別体に設けても良い。なお、エンドエフェクタ２を三次元的に目標Ｔの位置に移動制御する場合にも同様に適用できる。
The "target position sensor" and the "obstacle position sensor" have been shown in a form configured to detect the position of the obstacle O and the position of the target T by a
制御対象としてロボット１のエンドエフェクタ２を制御する実施形態を示したが、自律型移動ロボットにも適用でき、さらに、自動車、自動二輪車などの車両に適用することでいわゆる運転支援技術、自動運転技術にも適用できる。すなわち、制御対象は、ロボット１のエンドエフェクタ２に限られず、車両などにも適用でき、各種の移動体に適用できる。
Although the embodiment of controlling the
前述した実施形態の各要素を組み合わせて構成しても良い。前述実施形態に記述した各機能をハードウェアにより実現してもハードウェア及びソフトウェアの組合せにより実現しても良い。また、特許請求の範囲に記載した括弧内の符号は、本発明の一つの態様として前述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。前述実施形態の一部を、課題を解決できる限りにおいて省略した態様も実施形態と見做すことが可能である。また、特許請求の範囲に記載した文言によって特定される発明の本質を逸脱しない限度において、考え得るあらゆる態様も実施形態と見做すことが可能である。 Each element of the above-described embodiment may be combined and configured. Each function described in the above-described embodiment may be realized by hardware or by a combination of hardware and software. Further, the reference numerals in parentheses described in the claims indicate the correspondence with the specific means described in the above-described embodiment as one aspect of the present invention, and the technical scope of the present invention is defined. It is not limited. An embodiment in which a part of the above-described embodiment is omitted as long as the problem can be solved can also be regarded as an embodiment. In addition, any conceivable embodiment can be regarded as an embodiment as long as it does not deviate from the essence of the invention specified by the wording described in the claims.
本開示は、前述した実施形態に準拠して記述したが、当該実施形態や構造に限定されるものではないと理解される。本開示は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範畴や思想範囲に入るものである。 Although the present disclosure has been described in accordance with the above-described embodiment, it is understood that the present disclosure is not limited to the embodiment or structure. The present disclosure also includes various variations and variations within a uniform range. In addition, various combinations and forms, as well as other combinations and forms, including one element, more, or less, are within the scope and scope of the present disclosure.
図面中、２はエンドエフェクタ（移動体）、３，１０３は制御システム、１０，１１０は制御装置、１１はセンサ（目標位置センサ、障害物位置センサ）、１２はロボットジョイントコントローラ（コントローラ）、１４は到達モジュール（到達制御値出力部）、１５は回避モジュール（回避制御値出力部）、１６は混合モジュール（混合制御値算出部）、１１６は混合モジュール（混合制御値算出部、動力発生制御値出力部）、１７は計時モジュール（計時部）、Ｏは障害物、Ｔは目標、Ｃａは回避制御値、Ｃｒは到達制御値、Ｃｍは混合制御値（制御値）、を示す。 In the drawing, 2 is an end effector (moving body), 3,103 is a control system, 10 and 110 are control devices, 11 is a sensor (target position sensor, obstacle position sensor), 12 is a robot joint controller (controller), and 14 Is the arrival module (arrival control value output unit), 15 is the avoidance module (avoidance control value output unit), 16 is the mixing module (mixing control value calculation unit), and 116 is the mixing module (mixing control value calculation unit, power generation control value). Output unit), 17 is a time measurement module (time measurement unit), O is an obstacle, T is a target, Ca is an avoidance control value, Cr is an arrival control value, and Cm is a mixing control value (control value).
Claims (9)
目標位置センサ（１１）により取得される目標の位置に応じて当該目標の位置に前記移動体を到達させるための到達制御値（Ｃｒ）を出力する到達制御値出力部（１４）と、
障害物位置センサ（１１）により取得される障害物の位置に応じて前記移動体の前記障害物への衝突を回避するための回避制御値（Ｃａ）を出力する回避制御値出力部（１５）と、
前記到達制御値出力部の到達制御値、前記回避制御値出力部の回避制御値、前記到達制御値の重み付け値、及び、前記回避制御値の重み付け値に応じて混合制御値（Ｃｍ）を算出する混合制御値算出部（１６）と、を備え、
前記混合制御値算出部は、前記移動体と前記障害物との間の距離が所定の閾値以下の場合、前記距離が大きくなると単純減少するように前記回避制御値の重み付け値を入力することで前記距離が閾値よりも近いときには前記混合制御値を回避制御方向の値とし、前記移動体と前記障害物との間の前記距離が所定の閾値以上の場合、前記距離が大きくなると単純増加するように前記到達制御値の重み付け値を入力することで前記距離が閾値よりも遠いときには前記混合制御値を到達制御方向の値とするものであり、
前記混合制御値算出部は、前記移動体と前記障害物との間の前記距離が所定の閾値と等しいときには前記回避制御値の重み付け値を０とし、前記到達制御値の重み付け値を０とし、前記回避制御値の重み付け値の微分値を０とし、且つ、前記到達制御値の重み付け値の微分値を０とする、移動体の制御装置。 It is a moving body control device (10, 110) that controls the moving body (2) to be controlled to the position of the target (T) while avoiding the obstacle (O).
The arrival control value output unit (14) that outputs the arrival control value (Cr) for reaching the moving body at the target position according to the target position acquired by the target position sensor (11), and the arrival control value output unit (14).
The avoidance control value output unit (15) that outputs the avoidance control value (Ca) for avoiding the collision of the moving object with the obstacle according to the position of the obstacle acquired by the obstacle position sensor (11). When,
A mixed control value (Cm) is calculated according to the arrival control value of the arrival control value output unit, the avoidance control value of the avoidance control value output unit, the weighted value of the reached control value, and the weighted value of the avoidance control value. The mixing control value calculation unit (16) is provided.
When the distance between the moving body and the obstacle is equal to or less than a predetermined threshold value, the mixed control value calculation unit inputs a weighted value of the avoidance control value so as to simply decrease as the distance increases. When the distance is closer than the threshold value, the mixed control value is set as the value in the avoidance control direction, and when the distance between the moving body and the obstacle is equal to or more than a predetermined threshold value, the distance is simply increased as the distance increases. By inputting the weighted value of the arrival control value in, when the distance is farther than the threshold value, the mixed control value is set as the value in the arrival control direction.
When the distance between the moving body and the obstacle is equal to a predetermined threshold value, the mixed control value calculation unit sets the weighting value of the avoidance control value to 0 and the weighting value of the arrival control value to 0. , A moving body control device in which the differential value of the weighted value of the avoidance control value is 0, and the differential value of the weighted value of the reached control value is 0 .
前記移動体と前記障害物との相対距離ベクトルのスカラー値（｜→Ｄ｜）が所定の閾値（Ｄ０）を含みそれ以上のときには、前記混合制御値算出部は、前記到達制御値出力部の到達制御値（Ｃｒ）に０を超え１までの重み付け値を乗算した混合制御値とし、
前記移動体と前記障害物の相対距離ベクトルのスカラー値（｜→Ｄ｜）が前記所定の閾値（Ｄ０）より小さいとき、前記混合制御値算出部は、前記移動体と前記障害物の相対速度ベクトルと前記相対距離べクトルとの内積値（「→Ｖ」・「→Ｄ」）が０以上となるときに前記到達制御値出力部の到達制御値（Ｃｒ）に０を超え１までの重み付け値を乗算した混合制御値とし、前記内積値が０未満となるときには前記回避制御値出力部の回避制御値（Ｃａ）に０を超え１までの重み付け値を乗算した混合制御値とする、請求項１から３の何れか一項に記載の移動体の制御装置。 The mixing control value calculation unit is
When the scalar value (| → D |) of the relative distance vector between the moving body and the obstacle includes a predetermined threshold value (D0) and is higher than that, the mixed control value calculation unit is the arrival control value output unit. A mixed control value obtained by multiplying the arrival control value (Cr) by a weighted value exceeding 0 and up to 1.
When the scalar value (| → D |) of the relative distance vector between the moving body and the obstacle is smaller than the predetermined threshold value (D0), the mixing control value calculation unit determines the relative speed between the moving body and the obstacle. When the inner product value (“→ V” / “→ D”) of the vector and the relative distance vector becomes 0 or more, the arrival control value (Cr) of the arrival control value output unit is weighted from 0 to 1 A mixed control value obtained by multiplying the values, and when the inner product value is less than 0, the mixed control value obtained by multiplying the avoidance control value (Ca) of the avoidance control value output unit by a weighted value exceeding 0 and up to 1 is claimed . Item 6. The moving body control device according to any one of Items 1 to 3 .
前記計時部により一定領域内に留まる時間を計時することで前記障害物（Ｏ）が静止障害物と判定されたときには、前記障害物の位置と前記移動体の位置に基づいて当該移動体に動力を発生させる制御値を出力することで前記移動体と前記静止障害物との相対位置関係の硬直状態を解消する動力発生制御値出力部（１１６）と、
を備える請求項１から４の何れか一項に記載の移動体の制御装置。 A time measuring unit (17) for measuring the time that the obstacle stays in a certain area is provided.
When the obstacle (O) is determined to be a stationary obstacle by measuring the time of staying in a certain area by the time measuring unit, the moving body is powered based on the position of the obstacle and the position of the moving body. The power generation control value output unit (116) that eliminates the rigid state of the relative positional relationship between the moving body and the stationary obstacle by outputting the control value that generates
The moving body control device according to any one of claims 1 to 4 .
前記動力発生制御値出力部は、
（「→Ｄ２」－「→Ｄ１」）・「→Ｆ」＝０ …（１）
（「→Ｄ０」－「→Ｄ１」）・「→Ｆ」≧０ …（２）
の（１）（２）式を満たすように前記移動体に動力を発生させる制御値を出力する請求項５から７の何れか一項に記載の移動体の制御装置。 The vector of the target position, the position of the moving body, and the position of the obstacle are "→ D0", "→ D1", and "→ D2", respectively, and the vector of the force applied to the moving body is "→ F". When
The power generation control value output unit is
("→ D2"-"→ D1") ・ "→ F" = 0 ... (1)
(“→ D0”-“→ D1”) ・ “→ F” ≧ 0… (2)
The control device for a moving body according to any one of claims 5 to 7 , which outputs a control value for generating power to the moving body so as to satisfy the equations (1) and (2).
目標（Ｔ）の位置を取得する目標位置センサ（１１）と、
障害物（Ｏ）の位置を取得する障害物位置センサ（１１）と、
前記制御装置による混合制御値を入力し前記移動体を動作させるコントローラ（１２）と、を備える移動体の制御システム。 The control device (10, 110) according to any one of claims 1 to 8 .
A target position sensor (11) that acquires the position of the target (T), and
The obstacle position sensor (11) that acquires the position of the obstacle (O) and
A moving body control system including a controller (12) for inputting a mixing control value by the control device and operating the moving body.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017113380A JP7024215B2 (en) | 2017-06-08 | 2017-06-08 | Mobile control device and control system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017113380A JP7024215B2 (en) | 2017-06-08 | 2017-06-08 | Mobile control device and control system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2018202579A JP2018202579A (en) | 2018-12-27 |
| JP7024215B2 true JP7024215B2 (en) | 2022-02-24 |
Family
ID=64955878
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2017113380A Active JP7024215B2 (en) | 2017-06-08 | 2017-06-08 | Mobile control device and control system |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7024215B2 (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112914601B (en) * | 2021-01-19 | 2024-04-02 | 深圳市德力凯医疗设备股份有限公司 | Obstacle avoidance method and device for mechanical arm, storage medium and ultrasonic equipment |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008021178A (en) | 2006-07-13 | 2008-01-31 | Fujitsu Ltd | Autonomous mobile robot, autonomous locomotion method and autonomous locomotion information processing program |
| JP2008238395A (en) | 2007-03-23 | 2008-10-09 | Honda Research Inst Europe Gmbh | Robot with collision avoidance functionality |
| JP2016055404A (en) | 2014-09-12 | 2016-04-21 | キヤノン株式会社 | Locus generation method, locus generation device, robot device, program, and recording medium |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3210121B2 (en) * | 1992-02-10 | 2001-09-17 | 本田技研工業株式会社 | Obstacle avoidance route search method for moving objects |
| JPH05297944A (en) * | 1992-04-24 | 1993-11-12 | Fujitsu Ltd | Obstacle avoiding system for mobile robot |
| JPH06149357A (en) * | 1992-10-30 | 1994-05-27 | Sony Corp | Learning method and guidance method for path |
| JP3513989B2 (en) * | 1995-07-24 | 2004-03-31 | 株式会社安川電機 | Command interpolation method for positioning control system |
| JPH09185412A (en) * | 1995-12-28 | 1997-07-15 | Yaskawa Electric Corp | Autonomous moving device |
| JP3529575B2 (en) * | 1997-02-17 | 2004-05-24 | 株式会社東芝 | Force control robot and control method thereof |
| JPH1133961A (en) * | 1997-07-25 | 1999-02-09 | Nippon Telegr & Teleph Corp <Ntt> | Method and device for controlling robot manipulator |
-
2017
- 2017-06-08 JP JP2017113380A patent/JP7024215B2/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008021178A (en) | 2006-07-13 | 2008-01-31 | Fujitsu Ltd | Autonomous mobile robot, autonomous locomotion method and autonomous locomotion information processing program |
| JP2008238395A (en) | 2007-03-23 | 2008-10-09 | Honda Research Inst Europe Gmbh | Robot with collision avoidance functionality |
| JP2016055404A (en) | 2014-09-12 | 2016-04-21 | キヤノン株式会社 | Locus generation method, locus generation device, robot device, program, and recording medium |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2018202579A (en) | 2018-12-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9403275B2 (en) | Dynamic obstacle avoidance in a robotic system | |
| JP5978028B2 (en) | Mobile robot controller | |
| JP4243309B2 (en) | Robot arm control device | |
| KR102003216B1 (en) | Motor control and / or adjustment for the robot | |
| JP7067289B2 (en) | Motion planning device and motion planning method | |
| JP7024215B2 (en) | Mobile control device and control system | |
| JP7160118B2 (en) | Control device, control method, program | |
| JP2016062441A (en) | Omnidirectional transfer vehicle | |
| Kumagai et al. | Achievement of recognition guided teleoperation driving system for humanoid robots with vehicle path estimation | |
| Ariffin et al. | Sensor based mobile navigation using humanoid robot Nao | |
| Sartoretti et al. | Autonomous decentralized shape-based navigation for snake robots in dense environments | |
| Liu et al. | VPH+ and MPC Combined Collision Avoidance for Unmanned Ground Vehicle in Unknown Environment | |
| Endo et al. | Stability analysis for heterogeneous swarm robots with limited field of view | |
| Quyen Bui et al. | Sonar-based obstacle avoidance using region partition scheme | |
| Motoi et al. | Remote control method with force assist based on collision prediction calculated from each turning radius in mobile robot | |
| Zuhair et al. | TrackerBot: Parallel Object Tracking and Collision Avoidance Design for Unmanned Ground Vehicles | |
| Sakti et al. | Path planning and path following using arrival time field for nonholonomic mobile robot | |
| KR101957742B1 (en) | Real-time obstacle avoidance method of non-holonomic mobile robot using expanded guide circle, and system for controlling non-holonomic mobile robot remotely implementing the same | |
| Ariffin et al. | Real-time obstacle avoidance for humanoid-controlled mobile platform navigation | |
| JP4289501B2 (en) | Robot control method and program | |
| Kobayashi et al. | Tracking Control Method Considering Obstacle Avoidance by Reflective Motion for Mobile Robot | |
| Miah et al. | Intelligent networked navigation of mobile robots with collision avoidance | |
| Abbass | Design of Mobile Robot for Obstacle Avoidance using Microcontroller | |
| Li et al. | Development of the KNRm System Based Mobile Robot | |
| Shim et al. | A study on travelling control of humanoid type mobile robot with three wheel |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200515 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210317 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20210427 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20210622 |
|
| 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: 20220111 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20220124 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 7024215 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |