JP2016120561A - Transfer robot and transfer robot control method - Google Patents
Transfer robot and transfer robot control method Download PDFInfo
- Publication number
- JP2016120561A JP2016120561A JP2014262031A JP2014262031A JP2016120561A JP 2016120561 A JP2016120561 A JP 2016120561A JP 2014262031 A JP2014262031 A JP 2014262031A JP 2014262031 A JP2014262031 A JP 2014262031A JP 2016120561 A JP2016120561 A JP 2016120561A
- Authority
- JP
- Japan
- Prior art keywords
- force
- turning
- translational
- change amount
- cart
- 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.)
- Granted
Links
- 238000012546 transfer Methods 0.000 title claims abstract description 55
- 238000000034 method Methods 0.000 title claims description 10
- 238000004364 calculation method Methods 0.000 claims abstract description 43
- 238000001514 detection method Methods 0.000 claims abstract description 28
- 238000013519 translation Methods 0.000 claims abstract description 23
- 230000008859 change Effects 0.000 claims description 147
- 238000005096 rolling process Methods 0.000 claims description 35
- 230000007613 environmental effect Effects 0.000 claims description 16
- 230000032258 transport Effects 0.000 description 26
- 238000006073 displacement reaction Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 10
- 230000001133 acceleration Effects 0.000 description 5
- 230000007246 mechanism Effects 0.000 description 5
- 230000009471 action Effects 0.000 description 4
- 230000005484 gravity Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000006399 behavior Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000007935 neutral effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 210000000078 claw Anatomy 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Images
Abstract
Description
本発明は、カートを搬送する搬送ロボット及びその制御方法に関するものである。 The present invention relates to a transport robot for transporting a cart and a control method thereof.
周囲に物体との接触を検出するための複数のセンサを備える無人搬送車が知られている（特許文献１参照）。 An automatic guided vehicle having a plurality of sensors for detecting contact with an object around is known (see Patent Document 1).
ところで、例えば、搬送ロボットが家庭内などの狭い環境内でカート牽引し搬送を行う場合、カートが周囲の環境と接触する虞がある。これに対し、上記特許文献１のようにカート周囲に複数のセンサを設けることが考えられるが、システムが複雑となり、コスト増加を招く可能性がある。
By the way, for example, when the transport robot pulls and transports the cart in a narrow environment such as home, the cart may come into contact with the surrounding environment. On the other hand, it is conceivable to provide a plurality of sensors around the cart as in
本発明は、このような問題点を解決するためになされたものであり、簡易な構成で、カートが環境と接触したときのその接触力を緩和できる搬送ロボット及びその制御方法を提供することを主たる目的とする。 The present invention has been made to solve such problems, and provides a transport robot and a control method thereof that can reduce the contact force when a cart comes into contact with the environment with a simple configuration. Main purpose.
上記目的を達成するための本発明の一態様は、ロボット本体と、前記ロボット本体に設けられ、移動する移動手段と、前記ロボット本体に設けられ、カートを牽引し、並進方向及び旋回方向に移動可能なアームと、前記アームに対し並進方向に作用する並進力を検出する並進力検出手段と、前記アームに対し旋回方向に作用する旋回力を検出する旋回力検出手段と、を備える搬送ロボットであって、前記並進力検出手段により検出された並進力に基づいて、前記カートに対して外力が作用したときの前記アームに対する並進力の変化量を算出する並進力変化量算出手段と、前記旋回力検出手段により検出された旋回力に基づいて、前記カートに対して外力が作用したときの前記アームに対する旋回力の変化量を算出する旋回力変化量算出手段と、前記並進力変化量算出手段により算出された並進力の変化量及び前記旋回力変化量算出手段により算出された旋回力の変化量、のうちの少なくとも一方に倣うように倣い制御を実行する制御手段と、を備えることを特徴とする搬送ロボットである。
この一態様において、前記制御手段は、前記並進力変化量算出手段により算出された並進力の変化量が第１閾値以上、及び前記旋回力変化量算出手段により算出された旋回力の変化量が第２閾値以上、のうちの少なくとも一方となるとき、前記倣い制御を変更してもよい。
この一態様において、前記制御手段は、前記並進力変化量算出手段により算出された並進力の変化量が第１閾値以上、及び前記旋回力変化量算出手段により算出された旋回力の変化量が第２閾値以上、のうちの少なくとも一方となるとき、前記移動手段を制御して、前記カートを所定位置まで後退させ、前記カートの進行方向に対して、該カートに作用する外力の位置と反対側へ前記カートを所定量移動させる復帰制御を行なってもよい。
この一態様において、前記並進力変化量算出手段は、前記カートの各車輪の転がり抵抗に基づいて該カートに外力が作用していないときの並進力の想定値を算出し、前記並進力検出手段により検出された並進力と該算出した並進力の想定値とに基づいて前記並進力の変化量を算出し、前記旋回力変化量算出手段は、前記カートの各車輪の転がり抵抗に基づいて該カートに外力が作用していないときの旋回力の想定値を算出し、前記旋回力検出手段により検出された旋回力と該算出した旋回力の想定値とに基づいて前記旋回力の変化量を算出してもよい。
この一態様において、当該搬送ロボットが移動する環境内の領域毎に、前記転がり抵抗が設定された環境地図情報を記憶する記憶手段を更に備え、前記並進力変化量算出手段及び旋回力変化量算出手段は、前記記憶手段の環境地図情報と、当該搬送ロボットの現在位置情報と、に基づいて、前記転がり抵抗を算出し、該算出した転がり抵抗に基づいて、前記並進力の想定値及び前記旋回力の想定値を夫々算出してもよい。
この一態様において、前記制御手段は、前記並進力変化量算出手段により算出された並進力の変化量に基づいて、前記並進力のインピーダンス制御又はアドミタンス制御を行ない、前記旋回力変化量算出手段により算出された旋回力の変化量に基づいて、前記旋回力のインピーダンス制御又はアドミタンス制御を行なってもよい。
この一態様において、当該搬送ロボットが走行する斜面の傾斜を検出する傾斜検出手段を更に備え、前記並進力変化量算出手段は、前記傾斜検出手段により検出された前記斜面の傾斜に基づいて補正した前記並進力の想定値を算出し、該補正した並進力の想定値に基づいて前記並進力の変化量を算出し、前記旋回力変化量算出手段は、前記傾斜検出手段により検出された前記斜面の傾斜に基づいて補正した前記旋回力の想定値を算出し、該補正した旋回力の想定値に基づいて前記旋回力の変化量を算出してもよい。
この一態様において、当該搬送ロボットが移動する環境内の領域毎に、前記カートの各車輪の転がり抵抗が設定された環境地図情報を記憶する記憶手段を更に備え、前記制御手段は、前記記憶手段の環境地図情報と、当該搬送ロボットの現在位置情報と、に基づいて、前記第１及び第２閾値を変更してもよい。
上記目的を達成するための本発明の一態様は、ロボット本体と、前記ロボット本体に設けられ、移動する移動手段と、前記ロボット本体に設けられ、カートを牽引し、並進方向及び旋回方向に移動可能なアームと、を備える搬送ロボットの制御方法であって、前記アームに対し並進方向に作用する並進力を検出するステップと、前記アームに対し旋回方向に作用する旋回力を検出するステップと、前記検出された並進力に基づいて、前記カートに対して外力が作用したときの前記アームに対する並進力の変化量を算出するステップと、前記検出された旋回力に基づいて、前記カートに対して外力が作用したときの前記アームに対する旋回力の変化量を算出するステップと、前記算出された並進力の変化量及び前記算出された旋回力の変化量、のうちの少なくとも一方に倣うように倣い制御を実行するステップと、を含むことを特徴とする搬送ロボットの制御方法であってもよい。
In one aspect of the present invention for achieving the above object, a robot main body, a moving means provided in the robot main body and moving, and provided in the robot main body, pulling a cart and moving in a translational direction and a turning direction. A transfer robot comprising: a movable arm; a translational force detecting means for detecting a translational force acting on the arm in a translational direction; and a turning force detecting means for detecting a turning force acting on the arm in the turning direction. A translation force change amount calculating means for calculating a change amount of the translation force with respect to the arm when an external force is applied to the cart based on the translation force detected by the translation force detecting means; Based on the turning force detected by the force detecting means, a turning force change amount calculating means for calculating a change amount of the turning force with respect to the arm when an external force is applied to the cart. Control for executing copying control so as to follow at least one of the amount of change in translation force calculated by the amount-of-translation-force calculating unit and the amount of change in turning force calculated by the amount-of-turning force calculating unit And a transport robot.
In this aspect, the control means has a translational force change amount calculated by the translational force change amount calculating unit equal to or greater than a first threshold value, and a turning force change amount calculated by the turning force change amount calculating unit. The scanning control may be changed when at least one of the second threshold value and the second threshold value is reached.
In this aspect, the control means has a translational force change amount calculated by the translational force change amount calculating unit equal to or greater than a first threshold value, and a turning force change amount calculated by the turning force change amount calculating unit. When at least one of the second threshold value and the second threshold value is reached, the moving means is controlled to retract the cart to a predetermined position, opposite to the position of the external force acting on the cart with respect to the traveling direction of the cart. A return control for moving the cart to the side by a predetermined amount may be performed.
In this aspect, the translational force change amount calculating means calculates an estimated value of the translational force when no external force is acting on the cart based on the rolling resistance of each wheel of the cart, and the translational force detecting means The amount of change in the translational force is calculated based on the translational force detected by the vehicle and the estimated value of the calculated translational force, and the amount of change in the turning force is calculated based on the rolling resistance of each wheel of the cart. An assumed value of the turning force when no external force is applied to the cart is calculated, and the amount of change in the turning force is calculated based on the turning force detected by the turning force detecting means and the calculated estimated value of the turning force. It may be calculated.
In this aspect, storage means for storing environment map information in which the rolling resistance is set is further provided for each region in the environment in which the transfer robot moves, and the translational force change amount calculating unit and the turning force change amount calculating unit are further provided. The means calculates the rolling resistance based on the environmental map information of the storage means and the current position information of the transfer robot, and based on the calculated rolling resistance, the estimated value of the translation force and the turning The assumed force values may be calculated respectively.
In this aspect, the control unit performs impedance control or admittance control of the translational force based on the translational force variation calculated by the translational force variation calculation unit, and the turning force variation calculation unit Based on the calculated amount of change in turning force, impedance control or admittance control of the turning force may be performed.
In this aspect, the apparatus further includes an inclination detection unit that detects an inclination of the slope on which the transfer robot travels, and the translational force change amount calculation unit corrects based on the inclination of the slope detected by the inclination detection unit. An estimated value of the translational force is calculated, a change amount of the translational force is calculated based on the corrected estimated value of the translational force, and the turning force change amount calculating unit is configured to detect the slope detected by the inclination detecting unit. The assumed value of the turning force corrected based on the inclination of the turning force may be calculated, and the change amount of the turning force may be calculated based on the corrected assumed value of the turning force.
In this aspect, for each region in the environment in which the transfer robot moves, the storage robot further includes storage means for storing environment map information in which rolling resistance of each wheel of the cart is set, and the control means includes the storage means The first and second thresholds may be changed based on the environmental map information and current position information of the transfer robot.
In one aspect of the present invention for achieving the above object, a robot main body, a moving means provided in the robot main body and moving, and provided in the robot main body, pulling a cart and moving in a translational direction and a turning direction. An arm capable of detecting a translational force acting on the arm in a translational direction; and detecting a turning force acting on the arm in a pivoting direction; Based on the detected translational force, calculating a change amount of the translational force on the arm when an external force is applied to the cart; and on the cart based on the detected turning force A step of calculating a change amount of the turning force with respect to the arm when an external force is applied; a change amount of the calculated translation force; and a change amount of the calculated turning force; Performing a so scanning control so as to follow at least one of out, may be a method of controlling a transfer robot, which comprises a.
本発明によれば、簡易な構成で、カートが環境と接触したときのその接触力を緩和できる搬送ロボット及びその制御方法を提供することができる。 According to the present invention, it is possible to provide a transfer robot that can relieve the contact force when the cart comes into contact with the environment with a simple configuration, and a control method thereof.
実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の実施形態１に係る搬送ロボットの概略的構成の一例を示す斜視図である。本実施形態１に係る搬送ロボットは、例えば、図１に示すようにカートを牽引し自律的に搬送する自律型ロボットとして構成されている。
Embodiments of the present invention will be described below with reference to the drawings.
FIG. 1 is a perspective view showing an example of a schematic configuration of a transfer robot according to
図２は、本発明の実施形態１に係る搬送ロボットの概略的なシステム構成を示すブロック図である。本実施の形態１に係る搬送ロボット１は、ロボット本体２と、ロボット本体２を移動させる移動装置３と、カートを把持し牽引するアーム装置４と、移動装置３及びアーム装置４を制御する制御装置５と、環境情報を検出する環境センサ６と、各関節部のトルクを検出するトルクセンサ７と、各関節部の角度を検出する角度センサ８と、を備えている。
FIG. 2 is a block diagram illustrating a schematic system configuration of the transfer robot according to the first embodiment of the present invention. The
移動装置３は、制御装置５からの制御信号に応じて、例えば、モータなどを駆動させることで複数の車輪を回転させ、ロボット本体２を所望の位置に移動させる。移動装置３は、例えば、並進２自由度、および、旋回１自由度の制御自由度を有し、全方位へ移動可能なホロノミック台車として構成されている。移動装置３には、胴体関節部２１を介してロボット本体２が連結されている。ロボット本体２は、胴体関節部２１の関節軸（旋回軸）を中心にして、移動装置３に対して旋回方向に相対回動する（図３）。
The moving
胴体関節部２１には、胴体関節部２１を駆動するモータなどのアクチュエータ２２が設けられている。アクチュエータ２２は、制御装置５からの制御信号に応じて胴体関節部２１を駆動しロボット本体２を旋回させる。ロボット本体２には、アーム装置４が連結されている。したがって、アクチュエータ２２は、ロボット本体２を旋回させることでアーム装置４を旋回させることができる。
The
アーム装置４は、例えば、物体を把持する把持部４１と、把持部４１に第１関節部４２を介して連結される第１リンク４３と、第１リンク４３に第２関節部４４を介して連結される第２リンク４５と、各第１及び第２関節部４２、４４を駆動するモータなどのアクチュエータ４６と、第２リンク４５を昇降させる昇降機構４７と、からなる多関節型アームとして構成されている。把持部４１は、例えば、把持爪を開閉駆動することで物体を把持することができる。アーム装置４は、第１及び第２関節部４２、４４を回転させることで把持部４１を並進方向（搬送ロボット１の進行方向）に移動させる。アーム装置４は、昇降機構４７を昇降させることで、第２リンク４５を昇降させ把持部４１をカートのハンドル部の高さ位置に移動させる。そして、把持部４１は、カートのハンドル部を把持しハンドル部と連結する。なお、上記アーム装置４の構成は、一例でありこれに限定されない。また、ロボット本体２の胴体関節部２１が回転することで、ロボット本体２及びアーム装置４が旋回しているが、これに限定されない。アーム装置４の第２リンク４５が関節部を介してロボット本体２に連結され、この関節部を中心にしてアーム装置４が旋回してよい。
The
カートＸは、例えば、複数の車輪を有し、任意の方向へ移動可能なホロノミック台車として構成されている。また、把持部４１とカートＸのハンドル部とが非連結状態のときカートＸの各車輪は制動され、把持部４１とカートＸのハンドル部とが連結状態のとき、カートＸの各車輪は制動が解除されるように構成されている。
The cart X has, for example, a plurality of wheels and is configured as a holonomic carriage that can move in any direction. Each wheel of the cart X is braked when the
第１乃至２関節部４２、４４、及び胴体関節部２１には、各関節軸のトルクを検出するトルクセンサ７が夫々設けられている。トルクセンサ７は、検出した各関節軸のトルクを制御装置５に出力する。第１乃至２関節部４２、４４及び胴体関節部２１には、各関節軸の回転角を検出する角度センサ８が夫々設けられている。角度センサ８は、例えば、ポテンショメータ、ロータリーエンコーダなどで構成されている。角度センサ８は、検出した各関節軸の回転角を制御装置５に出力する。
The first and second
環境センサ６は、搬送ロボット１周囲に存在する障害物や操作対象物との距離などの環境情報を検出する。環境センサ６は、検出した環境情報を制御装置５に送信する。環境センサ６は、例えば、カメラ、超音波センサ、ミリ波センサなどの距離センサで構成されている。なお、上記搬送ロボット１の構成は、一例でありこれに限定されない。
The
制御装置５は、環境センサ６から出力される環境情報と、各トルクセンサ７から出力される各関節軸のトルクと、下記メモリなどに記憶された環境地図情報と、に基づいて、移動装置３およびアーム装置４のフィードバック制御などを行う。
Based on the environmental information output from the
なお、制御装置５は、例えば、演算処理、制御処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム、制御プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。
The
図４は、搬送ロボットがカートを牽引するときの状態を上方から見た概略図である。搬送ロボット１は、例えば、カートＸのハンドル部を把持し、牽引することでカートＸを搬送する。このとき、環境センサ６は、カートＸとは反対方向の進行方向の環境情報を検出する。搬送ロボット１とカートＸは、例えば、距離可変の状態で点線内で一体になって移動する。このとき、アーム装置４の把持部４１とカートＸのハンドル部との連結部Ｐ１には並進方向の並進力ｆが作用し、ロボット本体２の胴体関節部２１の関節軸（以下、旋回軸）Ｐ２周りには旋回方向の旋回トルクτが作用する。
FIG. 4 is a schematic view of the state when the transport robot pulls the cart as viewed from above. The
制御装置５は、並進力検出部５１と、旋回力検出部５２と、並進力変化量算出部５３と、旋回力変化量算出部５４と、制御部５５と、を備えている。
The
並進力検出部５１は、並進力検出手段の一具体例である。並進力検出部５１は、アーム装置４に対し並進方向に作用する並進力を算出する。並進力検出部５１は、例えば、アーム装置４の第１及び第２関節部４２、４４のトルクセンサ７から出力される各関節軸のトルクに基づいて、アーム装置４の把持部４１とカートＸのハンドル部との連結部Ｐ１に対して並進方向に作用する並進力ｆを算出する。並進力検出部５１は、算出した並進力ｆを並進力変化量算出部５３に出力する。
The translational
旋回力検出部５２は、旋回力検出手段の一具体例である。旋回力検出部５２は、アーム装置４に対し旋回方向に作用する旋回力を検出する。旋回力検出部５２は、例えば、ロボット本体２の胴体関節部２１のトルクセンサ７から出力される関節軸のトルクに基づいて、アーム装置４に対して旋回方向に作用する旋回力である、ロボット本体２の胴体関節部２１の旋回軸周りの旋回トルクτを算出する。旋回力検出部５２は、算出した旋回トルクτを旋回力変化量算出部５４に出力する。
The turning
並進力変化量算出部５３は、並進力変化量算出手段の一具体例である。並進力変化量算出部５３は、並進力検出部５１から出力される並進力ｆに基づいて、カートＸに対して外力が作用したときのアーム装置４に対する並進力の変化量（以下、並進力変化量）Δｆを算出する。並進力変化量算出部５３は算出した並進力変化量Δｆを制御部５５に出力する。
The translational force change
旋回力変化量算出部５４は、旋回力変化量算出手段の一具体例である。旋回力変化量算出部５４は、旋回力検出部５２から出力される旋回トルクτに基づいて、カートＸに対して外力が作用したときのアーム装置４に対する旋回力の変化量（以下、旋回力変化量）τΔを算出する。旋回力変化量算出部５４は算出した旋回力変化量Δτを制御部５５に出力する。
The turning force change
制御部５５は、制御手段の一具体例である。制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆと、旋回力変化量算出部５４により算出された旋回力変化量Δτと、に基づいて、これら並進力変化量Δｆ及び旋回力変化量Δτに倣うように、第１乃至第２関節部４２、４４及び胴体関節部２１のアクチュエータ４６と、昇降機構４７と、を制御する。
The
なお、並進力検出部５１及び旋回力検出部５２は、第１乃至第２関節部４２、４４及び胴体関節部２１に設けられたトルクセンサ７のトルクに基づいて、並進力ｆおよび旋回トルクτを算出しているが、これに限定されない。並進力検出部５１及び旋回力検出部５２は、第１乃至第２関節部４２、４４及び胴体関節部２１に設けられた電流センサ（モータの駆動電流を検出）、把持部４１に設けられた力覚センサ（６軸力力覚センサなど）、ロードセルセンサ、バネ変位センサ、などのセンサ値に基づいて、並進力ｆおよび旋回トルクτを算出してもよい。
Note that the translational
ここで、例えば、搬送ロボット１がカートＸを牽引し一定速度で並進移動する場合を想定する。連結部Ｐ１に作用する並進力ｆは、図５に示す如く、カートＸの各車輪の転がり抵抗ｆｒ１〜ｆｒ４の並進方向成分（ｘ軸方向成分）の和となり、下記式で算出できる。
ｆ＝ｆｒ＝ｆｒ１ｘ＋ｆｒ２ｘ＋ｆｒ３ｘ＋ｆｒ４ｘ
ロボット本体２の胴体関節部２１の旋回軸Ｐ２周りの旋回トルクτは、τ＝０となる。
Here, for example, it is assumed that the
f = f r = f r1x + f r2x + f r3x + f r4x
The turning torque τ around the turning axis P2 of the body
搬送ロボット１がカートＸを牽引し速度を変化させて並進移動する場合、連結部Ｐ１に作用する並進力ｆは、慣性力ｆｉ＝ｍａと転がり抵抗ｆｒとの和となり、下記式で算出できる。但し、積載物を含むカートＸの質量をｍとし、並進加速度をａとする。
ｆ＝ｆｉ＋ｆｒ
ロボット本体２の胴体関節部２１の旋回軸Ｐ２周りの旋回トルクτは、τ＝０となる。
If the
f = f i + f r
The turning torque τ around the turning axis P2 of the body
搬送ロボット１がカートＸを牽引し一定角速度で旋回移動する場合を想定する。胴体関節部２１の旋回軸Ｐ２とカートＸの各車輪を結んだ線分をｌ１〜ｌ４とし、その各線分の方向を第２軸、第２軸と直交する軸を第１軸とする（図６）。なお、線分ｌ１〜ｌ４は、カートＸの各車輪の取付位置及び搬送ロボット１の寸法などに基づいて求めることができる。カートＸの各車輪の転がり抵抗ｆｒ１〜ｆｒ４を第１及び第２軸の成分に分解したとき、第１軸方向の成分が旋回方向に作用する。このため、転がり抵抗による旋回軸Ｐ２の旋回トルクτｒは、下記式で表すことができる。
τｒ＝ｌ１ｆｒ１１＋ｌ２ｆｒ２１＋ｌ３ｆｒ３１＋ｌ４ｆｒ４１
並進力ｆはｆｒのｘ成分により相殺されるため、ｆ＝ｆｒ＝０となる。
Assume that the
τ r = l 1 f r11 + l 2 f r21 + l 3 f r31 + l 4 f r41
Translational force f is to be offset by the x component of f r, a f = f r = 0.
搬送ロボット１がカートＸを牽引し角速度を変化させて旋回移動する場合を想定する。胴体関節部２１の旋回軸Ｐ２周りのカートＸの慣性モーメントをＩとし、角速度をωとする。胴体関節部２１の旋回軸Ｐ２の旋回トルクτは、慣性モーメントτｉ＝Ｉ（ｄω／ｄｔ）と、転がり抵抗によるトルクτｒと、の和となり、下記式で表すことができる。なお、下記式において、説明を簡略化するために遠心力およびコリオリ力を省略している。
τ＝τｉ＋τｒ、ｆ＝０
It is assumed that the
τ = τ i + τ r , f = 0
搬送ロボット１がカートＸを牽引し速度を変化させて並進移動しつつ、角速度を変化させて旋回移動する場合は、並進力ｆ及び旋回トルクτは下記式で表すことができる。
ｆ＝ｆｉ＋ｆｒ、τ＝τｉ＋τｒ
When the
f = f i + f r , τ = τ i + τ r
ここで、搬送ロボット１が一定加速度ａ１、ａ２で動作したときの並進力ｆ１、ｆ２は、下記式により求めることができる。
ｆ１＝ｍａ１＋ｆｒ
ｆ２＝ｍａ２＋ｆｒ
ｍ＝（ｆ１−ｆ２）／（ａ１−ａ２）
Here, the translational forces f 1 and f 2 when the
f 1 = ma 1 + f r
f 2 = ma 2 + f r
m = (f 1 −f 2 ) / (a 1 −a 2 )
搬送ロボット１が一定速度で並進移動したときの並進力ｆ（＝ｆｒ）と、カートＸの４つの各車輪の転がり抵抗ｆｒ１〜ｆｒ４とが、簡略化のために同一と仮定する。この場合、カートＸの各車輪の転がり抵抗ｆｒ１〜ｆｒ４をｆｒ／４として同定できる。カートＸの各車輪への荷重Ｆ１〜Ｆ４を簡略化のため同一と仮定する。カートＸの各車輪の転がり抵抗係数をＣｒとすると、例えば、カートＸの各車輪の転がり抵抗ｆｒｉ＝ＣｒＦｉ、Ｆｉ＝ｍｇ／４（ｉ＝１〜４）が成立し、カートＸの各車輪の転がり抵抗係数Ｃｒを下記式のように同定できる。
Ｃｒ＝ｆｒｉ／Ｆｉ
＝（ｆｒ／４）／（ｍｇ／４）
＝ｆｒ／ｍｇ
For the sake of simplicity, it is assumed that the translational force f (= f r ) when the
C r = f ri / F i
= (F r / 4) / (mg / 4)
= F r / mg
以上から、搬送ロボット１が所望の移動動作を行うときの（環境と非接触で移動動作を行うときの）必要な並進力の想定値（以下、並進力想定値）ｆ′は下記式を用いて求めることができる。
ｆ′＝ｆｉ＋ｆｒ
＝ｍａ＋Ｃｒｍｇ
From the above, the assumed value of translational force (hereinafter referred to as translational force assumed value) f ′ required when the
f ′ = f i + f r
= Ma + C r mg
なお、搬送ロボット１が所望の移動動作を行うときの必要な旋回トルクの想定値（旋回力想定値）τ′についても上記並進力想定値ｆ′と同様にして求めることができる。並進力変化量算出部５３は、上記式を用いて、搬送ロボット１が所望の移動動作を行うときに必要な並進力想定値ｆ′を算出する。同様に、旋回力変化量算出部５４は、上記式を用いて、搬送ロボット１が所望の移動動作を行うときに必要な旋回力想定値τ′を算出する。
Note that the assumed value of the turning torque (turning force assumption value) τ ′ required when the
例えば、図７に示すように、搬送ロボット１がカートＸを牽引する際に、カートＸが環境に接触しカートＸに対し外力ｆｅが作用した場合を想定する。
For example, as shown in FIG. 7, it is assumed that when the
このとき、並進力検出部５１は、アーム装置４の第１及び第２関節部４２、４４のトルクセンサ７から出力される各関節軸のトルクに基づいて、連結部Ｐ１の並進力ｆｓを算出する。並進力変化量算出部５３は、並進力検出部５１から出力される並進力ｆｓと、上記算出した並進力想定値ｆ′と、に基づいて、式（ｆｓ＝ｆ′＋Δｆ、Δｆ＝ｆｅ ）を用いて、並進力変化量Δｆを算出する。なお、この場合、並進力変化量Δｆは、外力ｆｅによる並進力の変化量である。
In this case, the
同様に、旋回力検出部５２は、ロボット本体２の胴体関節部２１のトルクセンサ７から出力される胴体関節部２１の関節軸のトルクに基づいて、胴体関節部２１の旋回トルクτを算出する。旋回力変化量算出部５４は、旋回力検出部５２から出力される旋回トルクτと、上記算出した旋回力想定値τ′と、に基づいて、式（τ＝τ′＋Δτ、Δτ＝−ｌｅｆｅ）を用いて、旋回力変化量Δτを算出する。なお、旋回力変化量Δτは、外力ｆｅによって変化する旋回トルクの変化量である。
Similarly, the turning
カートＸに外力ｆｅが作用した場合に、上述のように、並進力変化量Δｆ及び旋回力変化量Δτが発生する。そして、制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆと、旋回力変化量算出部５４により算出された旋回力変化量Δτと、に基づいて、ロボット本体２の胴体関節部２１を旋回方向へかつアーム装置４の第１及び第２関節部４２、４４を並進方向へコンプライアントに制御（倣い制御、コンプライアント制御）する。すなわち、並進力変化量Δｆ及び旋回トルク変化量Δτが発生すると、それらに倣うように、アーム装置４の第１及び第２関節部４２、４４の並進自由度、および、胴体関節部２１の旋回自由度を制御する。これにより、外力ｆｅによって生じた並進力変化量Δｆ及び旋回トルク変化量Δτが過大にならないように抑制でき、環境との接触時に生じる接触力を緩和することができる。さらに、上記コンプライアント制御は、カートＸに特別なセンサなどを設ける必要が無いため、搬送ロボット１の構成を簡素化できる。すなわち、簡易な構成で、カートＸが環境と接触したときのその接触力を緩和できる。
When the external force fe is applied to the cart X, the translational force change amount Δf and the turning force change amount Δτ are generated as described above. Based on the translational force change amount Δf calculated by the translational force change
制御部５５は、上記コンプライアント制御として、例えば、インピーダンス制御やアドミタンス制御を行なう。これにより、カートＸの仮想的な慣性が小さくなるように制御でき、上記のようなカートＸの慣性によって生じる環境との接触力を十分に小さくできる。
The
次に、制御部が上記実行するインピーダンス制御の一例について詳細に説明する。図８は、並進力のインピーダンス制御のブロック線図である。図９は、並進力のインピーダンス制御の模式図である。 Next, an example of impedance control executed by the control unit will be described in detail. FIG. 8 is a block diagram of translational force impedance control. FIG. 9 is a schematic diagram of translational force impedance control.
ここで、仮想的な慣性をＭ、仮想的なバネ係数をＫｆ１、外力に倣う仮想力係数をＫｆ２、仮想的なダンパー係数をＤｆ、並進変位量（第２関節部４４と連結部Ｐ１間の距離）をｌｃ、並進変位の中立位置（連結部Ｐ１に力が作用していないときの第２関節部４４と連結部Ｐ１間の距離）をｌｃ０、仮想制御力をｆｖ、目標のインピーダンス挙動となる変位目標値（第２関節部４４と連結部Ｐ１間の目標距離）をｌｃ ｄ、として設定する。なお、制御部５３は、第１及び第２関節部４２、４４の角度センサ８から出力される各関節軸の回転角と、第１及び第２リンク４３、４５の寸法と、第２リンク４５の高さ位置と、に基づいて、上記並進変位量ｌｃを算出する。
Here, the virtual inertia is M, the virtual spring coefficient is K f1 , the virtual force coefficient following the external force is K f2 , the virtual damper coefficient is D f , and the translational displacement (the second
制御部５５は、仮想バネによる仮想復元力ｆ１を下記式により算出する。
ｆ１＝Ｋｆ１Δｌｃ＝Ｋｆ１（ｌｃ０−ｌｃ）
制御部５５は、外力に倣う仮想倣い力ｆ２を下記式より算出する。
ｆ２＝Ｋｆ２Δｆ＝Ｋｆ２（ｆｓ−ｆ′）
制御部５５は、仮想ダンパーによる粘性力ｆｄを下記式より算出する。
ｆｄ＝Ｄｆｄｌｃ／ｄｔ
f 1 = K f1 Δl c = K f1 (l c0 -l c)
f 2 = K f2 Δf = K f2 (f s -f ')
f d = D f dl c / dt
制御部５５は、上記算出した仮想復元力ｆ１、仮想倣い力ｆ２、及び粘性力ｆｄに基づいて仮想制御力ｆｖを下記式より算出する。
ｆｖ＝ｆ１＋ｆ２−ｆｄ
The
f v = f 1 + f 2 −f d
運動力学から仮想制御力ｆｖ＝Ｍｄ２ｌｃ／ｄｔ２の関係が成立する。したがって、制御部５５は、（並進変位量ｌｃの加速度）＝ｆｖ／Ｍとなるように並進変位量ｌｃを制御する。なお、制御部５５は、図８の（１）のクローズドループの制御系（例えば、位置のＰＩＤ制御）を用いて、並進変位量ｌｃの制御を行なう。以上のインピーダンス制御を行なうことで、並進力変化量Δｆが発生すると、その並進力変化量Δｆに倣ってアーム装置４の第１及び第２関節部４２、４４が動作する。このため、過大な並進方向の外力の作用を避けることができる。
From the kinematics, the relationship of virtual control force f v = Md 2 l c / dt 2 is established. Accordingly, the
制御部５５は、旋回トルクのインピーダンス制御も上記並進力のインピーダンス制御と同様に実行する。図１０は、旋回トルクのインピーダンス制御のブロック線図である。図１１は、旋回トルクのインピーダンス制御の模式図である。
The
ここで、仮想的な慣性モーメントをJ、仮想的なねじりバネ係数をＫτｌ、外力トルクに倣う仮想トルク係数をＫτ２、仮想的なねじりダンパー係数をＤτ、旋回変位量をθｃ、旋回変位の中立位置をθｃ０、仮想制御トルクをτｖ、目標のインピーダンス挙動となる変位目標値をθｃ ｄ、旋回力検出部５２から出力される胴体関節部２１の旋回トルクをτｓ、として設定する。なお、制御部５５は、胴体関節部２１の角度センサ８から出力される関節軸の回転角に基づいて、上記旋回変位量θｃを算出する。
Here, the virtual moment of inertia is J, the virtual torsion spring coefficient is K τl , the virtual torque coefficient following the external force torque is K τ2 , the virtual torsional damper coefficient is D τ , the turning displacement is θ c , the turning The neutral position of the displacement is θ c0 , the virtual control torque is τ v , the displacement target value that is the target impedance behavior is θ c d , and the turning torque of the body
制御部５５は、仮想ねじりバネによる仮想復元力τ１を下記式により算出する。
τ１＝Ｋτ１Δθｃ＝Ｋτ１（θｃ０−θｃ）
制御部５５は、外力トルクに倣う仮想倣いトルクτ２を下記式より算出する。
τ２＝Ｋτ２Δτ＝Ｋτ２（τｓ−τ′）
制御部５５は、仮想ねじりダンパーによる粘性トルクτｄを下記式より算出する。
τｄ＝Ｄτｄθｃ／ｄｔ
The
τ 1 = K τ1 Δθ c = K τ1 (θ c0 −θ c )
The
τ 2 = K τ2 Δτ = K τ2 (τ s -τ ')
The
τ d = D τ dθ c / dt
制御部５５は、上記算出した仮想復元力τ１、仮想倣いトルクτ２、及び粘性トルクτｄに基づいて仮想制御トルクτｖを下記式より算出する。
τｖ＝τ１＋τ２−τｄ
The
τ v = τ 1 + τ 2 −τ d
運動力学から仮想制御トルクτｖ＝Ｊｄ２θｃ／ｄｔ２の関係が成立する。制御部５５は、（アーム装置４の旋回変位量θｃの加速度）＝τｖ／Jとなるように旋回変位量θｃを制御する。なお、制御部５５は、図１０の（１）のクローズドループの制御系（例えば、位置のＰＩＤ制御）を用いて、並進変位量ｌｃの制御を行なう。これにより、旋回力変化量Δτが発生すると、その旋回力変化量Δτに倣ってロボット本体２の胴体関節部２１は動作する。このため、過大な旋回方向の外力の作用を避けることができる。
From the kinematics, the relationship of virtual control torque τ v = Jd 2 θ c / dt 2 is established.
制御部５５は、旋回力変化量Δτが発生すると、上記インピーダンス制御を実行してロボット本体２の胴体関節部２１を動作させ、カートＸを旋回させる。この場合、カートＸは、例えば、図７に示す環境に引っ掛った状態から、図１２に示すように、その引っ掛りを回避した状態となる。制御部５５は、図１２に示す如く、移動装置３を制御して進行方向へ真直ぐ牽引できる。なお、このとき、カートＸは進行方向に対して傾いた状態であり環境と接触している。このため、カートＸには外力が作用しており、この外力が牽引の抵抗となる。したがって、制御部５５は、移動装置３を制御して、図１３に示す如く、ロボット本体２を環境反対側の横方向へ移動させる経路補正を行う。これにより、カートＸは進行方向に向いた状態となり環境と接触しない状態となる。このため、搬送ロボット１は、環境からの接触抵抗を受けずにカートＸを牽引することができる。
When the turning force change amount Δτ is generated, the
以上、本実施形態１に係る搬送ロボット１において、制御装置５の制御部５５は、並進力変化量算出部５３により算出された並進力変化量及び旋回力変化量算出部５４により算出された旋回力変化量に倣うようにコンプライアント制御を実行する。これにより、簡易な構成で、カートＸが環境と接触したときのその接触力を緩和できる。
As described above, in the
実施形態２
並進力変化量Δｆ及び／又は旋回力変化量Δτが過大となる場合、例えば、カートＸが動けなくなるロック状態の可能性が高い。例えば、カートＸが環境と接触し、その接触摩擦力によりロック状態になる場合、カートＸの車輪が路面の段差に引っ掛りロック状態になる場合、カートＸが周辺の環境と多点で接触してロック状態になる場合などが想定される。制御装置５の制御部５５がそのまま上記倣い制御を継続しても、カートＸを牽引するのは困難である。
When the translational force change amount Δf and / or the turning force change amount Δτ is excessive, for example, there is a high possibility that the cart X is locked. For example, when the cart X comes into contact with the environment and is locked due to the frictional force of the contact, the cart X comes into contact with the surrounding environment at multiple points when the wheels of the cart X are locked by being caught on a step on the road surface. In such a case, it may be locked. Even if the
これに対し、本発明の実施形態２に係る制御装置５の制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆが第１閾値以上となり、及び／又は旋回力変化量算出部５４により算出された旋回力変化量Δτが第２閾値以上となるとき、コンプライアント制御を変更して、上記ロック状態を回避し復帰する制御（以下、復帰制御）を実行する。これにより、カートＸがロック状態であることを察知し、そのロック状態の回避を確実に行うことができる。
In contrast, in the
例えば、図１４に示す如く、制御部５５は、復帰制御において、旋回力変化量算出部５４により算出された旋回力変化量Δτの正負号に基づいて、外力ｆｅが旋回軸Ｐ２を通る進行方向に平行な線（以下、平行線）Ｌに対して上下どちら側に作用しているかを判断する。例えば、旋回力変化量Δτが時計方向であれば正とし、反時計方向を負とする。外力ｆｅは、平行線Ｌに対して上側に作用していると推定する。
For example, as shown in FIG. 14, in the return control, the
制御部５５は、移動装置３を制御して、並進力変化量Δｆ及び／又は旋回力変化量Δτが過大となる前の位置及び姿勢（以下、位置）Ｐ１まで、搬送ロボット１を後退させる（図１４）。この位置Ｐ１は、ロック状態を回避できる退避可能位置Ｐ１である。例えば、制御部５５は並進力変化量Δｆが第１閾値Δｆｍａｘよりも小さく、かつ、旋回力変化量Δτが第２閾値Δτｍａｘよりも小さくなるときの移動装置３の位置を予め記憶し、その記憶した位置を上記退避可能位置Ｐ１としている。
The
制御部５５は、移動装置３を制御して、平行線Ｌに対して外力ｆｅが作用している側と反対側方向（この場合、ｙ軸負方向）へ搬送ロボット１を所定量Δｙ移動させる（図１５）。なお、この所定量Δｙは、例えば、搬送ロボット１の移動環境、搬送ロボット１及びカートＸの寸法などに基づいて予め設定される。
The
制御部５５は、上記復帰制御を終了して、移動装置３を制御して搬送ロボット１及びカートＸを進行方向へ移動させる（図１６）。
The
図１８は、本実施形態２に係る制御部が実行する復帰制御のフローを示すフローチャートである。制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆが第１閾値以上であるか否かを判断する（ステップＳ２０１）。
FIG. 18 is a flowchart showing a flow of return control executed by the control unit according to the second embodiment. The
制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆが第１閾値Δｆｍａｘ以上であると判断したとき（ステップＳ２０１のＹＥＳ）、復帰制御を開始し、下記（ステップＳ２０３）の処理に移行する。一方、制御部５５は、並進力変化量算出部５３により算出された並進力変化量Δｆが第１閾値Δｆｍａｘよりも小さいと判断したとき（ステップＳ２０１のＮＯ）、旋回力変化量算出部５４により算出された旋回力変化量Δτが第２閾値Δτｍａｘ以上であるか否かを判断する（ステップＳ ２０２）。
制御部５５は、旋回力変化量算出部５４により算出された旋回力変化量Δτが第２閾値Δτｍａｘ以上であると判断したとき（ステップＳ２０２のＹＥＳ）、復帰制御を開始し、下記（ステップＳ２０３）の処理に移行する。
When the
制御部５５は、旋回力変化量算出部５４により算出された旋回力変化量Δτが０以上であるか否か（旋回力変化量Δτの正負号）を判断する（ステップＳ２０３）。
The
制御部５５は、旋回力変化量算出部５４により算出された旋回力変化量Δτが０以上（時計方向で正符号）であると判断すると（ステップＳ２０３のＹＥＳ）、移動装置３の制御量ΔＰ＝Δｙに設定する（ステップＳ２０４）。一方、制御部５５は、旋回力変化量算出部５４により算出された旋回力変化量Δτが０よりも小さい（反時計方向で負符号）であると判断すると（ステップＳ２０３のＮＯ）、移動装置３の制御量ΔＰ＝−Δｙに設定する（ステップＳ２０５）。
If the
制御部５５は、移動装置３の制御目標位置Ｐ′に退避可能位置Ｐ１を設定する。そして、制御部５５は、退避可能位置Ｐ１まで後退するように、移動装置３を制御する（ステップＳ２０６）。
制御部５５は、移動装置３の制御目標位置Ｐ′に（現在位置Ｐ＋制御量Δｙ）を設定することで、カートＸと環境との接触を回避する方向へ移動装置３を制御する（ステップＳ２０７）。
The
制御部５５は、旋回力変化量算出部５４により算出された旋回力変化量Δτが第２閾値Δτｍａｘよりも小さいと判断したとき（ステップＳ２０２のＮＯ）、搬送ロボット１の現在位置Ｐを退避可能位置Ｐ１として記憶する。
When the
なお、制御部５５は、並進力変化量Δｆが第１閾値Δｆｍａｘ以上となり、及び／又は旋回力変化量Δτが第２閾値Δτｍａｘ以上となるとき、復帰制御を実行しているが、これに限定されない。例えば、制御部５５は、並進力変化量Δｆが第１閾値Δｆｍａｘ以上となり、及び／又は旋回力変化量Δτが第２閾値Δτｍａｘ以上となるとき、移動装置３を減速させる制御を行なってもよい。さらに、制御部５５は、並進力変化量Δｆが第１閾値Δｆｍａｘ以上となり、及び／又は旋回力変化量Δτが第２閾値Δτｍａｘ以上となるとき、移動装置３を減速停止させる制御を行なってもよい。なお、制御部５５は、並進力変化量Δｆが第１閾値Δｆｍａｘ以上となり、及び／又は旋回力変化量Δτが第２閾値Δτｍａｘ以上となるとき、警告装置を用いてユーザに対して警告を行ってもよい。警告装置は、例えば、警告音を出力するスピーカ、警告灯を点灯／点滅させるライト装置、警告表示を行う表示装置、警告振動を発生する振動装置、管理者などに通知する通信装置などを含む。
The
実施形態３
本発明の実施形態３において、メモリに記憶された環境地図情報には、カートＸの各車輪の転がり抵抗係数Ｃｒが記憶されている。例えば、図１９に示すように、環境地図情報の領域毎には予め算出された転がり抵抗係数Ｃｒが設定されている。制御部５５は、メモリの環境地図情報と、搬送ロボット１の現在位置情報と、に基づいて、現在走行している領域の転がり抵抗Ｃｒを随時参照する。制御部５５は、参照した転がり抵抗係数Ｃｒに基づいて、並進力想定値ｆ′および旋回力想定値τ′を算出する。これにより、より高精度な転がり抵抗を用いて旋回力想定値τ′および並進力想定値ｆ′を算出できるため、より高精度に上記コンプライアント制御を実行できる。なお、制御部５５は、例えば、メモリの環境地図情報と、環境センサ６から出力される環境情報と、に基づいて、環境地図情報上における搬送ロボット１の現在位置を算出できる。
In a third embodiment of the present invention, the environment map information stored in the memory, the rolling resistance coefficient C r of the wheels of the cart X is stored. For example, as shown in FIG. 19, it is set in advance calculated rolling resistance coefficient C r in each area of the environment map information.
さらに、制御部５５は、メモリに記憶された転がり抵抗係数Ｃｒを含む環境地図情報と、搬送ロボット１の現在位置情報と、に基づいて、上記第１閾値Δｆｍａｘ及び／又は第２閾値Δτｍａｘを変更してもよい。例えば、制御部５５は、メモリに記憶された転がり抵抗係数Ｃｒを含む環境地図情報と、搬送ロボット１の現在位置情報と、に基づいて、搬送ロボット１が段差などの転がり抵抗係数Ｃｒが高い領域に進入すると判断すると、上記第１閾値Δｆｍａｘ及び／又は第２閾値Δτｍａｘを増加させるようにしてもよい。これにより、乗越え可能な段差を回避するなどの無駄な動作を抑制でき、効率的なカート運搬が可能となる。
Furthermore, the
実施形態４
本発明の実施形態４に係る制御装置５の制御部５５は、路面の傾斜角度θｓに基づいて、並進力想定値ｆ′および旋回力想定値τ′を補正する。これにより、搬送ロボット１が傾斜路面上でカートＸを牽引する場合でも、その傾斜路面による重力成分を考慮して、高精度にカートＸと環境との接触を推定し倣い制御が実行できる。
まず、並進力想定値ｆ′のみを補正する場合について説明する。例えば、搬送ロボット１が、図２０に示すような傾斜路面上でカートＸを牽引する場合を想定する。制御部５５は、搬送ロボット１のロボット本体２に設けられたジャイロセンサや加速度センサから出力されるセンサ値に基づいて、路面の傾斜角度θｓを算出できる。
First, the case where only the translational force assumption value f ′ is corrected will be described. For example, it is assumed that the
カートＸに作用する重力の並進力ｆｇは、ｆｇ＝ｍｇｓｉｎθｓとなる。また、路面に対し垂直な車輪荷重が傾斜により変化する。このため、カートＸの各車輪の転がり抵抗による作用力ｆｒ′は、傾斜のない場合の作用力ｆｒにｃｏｓθｓを乗算した値ｆｒ′＝ｆｒｃｏｓθｓとなる。 The gravity translational force f g acting on the cart X is f g = mg sin θ s . Further, the wheel load perpendicular to the road surface changes due to the inclination. Therefore, the action force f r by rolling of the wheels of the cart X ', the value f r multiplied by cos [theta] s to the action force f r in the absence of tilt' a = f r cos [theta] s.
したがって、並進力変化量算出部５３は、路面の傾斜角度θｓに基づいて下記式を用いて、補正後の並進力想定値ｆ′を算出できる。
ｆ′＝ｆｉ＋ｆｒ′＋ｆｇ
Therefore, the translational force change
f ′ = f i + f r ′ + f g
次に、並進力想定値ｆ′及び旋回力想定値τ′を補正する場合について説明する。例えば、図２１に示す如く、搬送ロボット１及びカートＸの方向と路面の傾斜方向が角度θｔとなる。なお、図２１は、上記搬送ロボットが傾斜角度θｓの傾斜路面上でカートＸを牽引する状態を上方から見た図である。
Next, the case where the translational force assumption value f ′ and the turning force assumption value τ ′ are corrected will be described. For example, as shown in FIG. 21, the inclination direction of the direction and the road surface of the
この場合、カートＸに作用する重力の並進力ｆｇ′＝ｆｇｃｏｓθｔとなる。
傾斜面での重力により作用する旋回トルクτ′ｇは、ｍｇｓｉｎθｓの旋回方向成分ｍｇｓｉｎθｓｓｉｎθｔと、 モーメントアームｌｇとに基づいて、下記式で算出できる。
τ′ｇ＝ｌｇｍｇｓｉｎθｓｓｉｎθｔ
In this case, the translational force f g '= f g cosθ t of gravity acting on the cart X.
Turning torque tau 'g which act by gravity on the inclined surface, and the turning direction component mgsinθ s sinθ t of Mgsinshita s, on the basis of the moment arm l g, it can be calculated by the following equation.
τ ′ g = l g mg sin θ s sin θ t
また、路面に対し垂直な車輪荷重が傾斜により変化する。このため、カートＸの各車輪の転がり抵抗による作用力τ′ｒは、傾斜のない場合の作用力τｒにｃｏｓθｓを乗算した値τｒ′＝τｒｃｏｓθｓとなる。 Further, the wheel load perpendicular to the road surface changes due to the inclination. Therefore, the acting force tau by rolling of the wheels of the cart X 'r is the value tau r multiplied by cos [theta] s to the working force tau r in the absence of tilt' a = τ r cosθ s.
以上から、並進力変化量算出部５３及び旋回力変化量算出部５４は、路面の傾斜角度θｓに基づいて、下記式を用いて、補正後の並進力想定値ｆ′及び旋回力想定値τ′を夫々算出できる。
並進力の想定値ｆ′＝ｆｉ＋ｆｒ′＋ｆｇ′
旋回トルクの想定値τ′＝τｉ＋τｒ′＋τｇ′
From the above, the translational force change
Assumed value of translational force f ′ = f i + f r ′ + f g ′
Assumed value of turning torque τ ′ = τ i + τ r ′ + τ g ′
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。 Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.
上記実施形態において、アーム装置の第１及び第２関節部をプーリあるいは平行リンク機構で連動させてもよい。これにより、駆動自由度を減らし、連結部の姿勢を一定に保つことができる。さらに、アーム装置は、第１リンクに第４関節部を介して第３リンクが連結され、第３リンクに第２関節部を介して第２リンクが連結される構成であってもよい。この場合、アーム装置は、第１、第２及び第４関節部を回転させることで、把持部を並進方向へ移動させることができる。 In the said embodiment, you may link the 1st and 2nd joint part of an arm apparatus with a pulley or a parallel link mechanism. Thereby, a drive freedom degree can be reduced and the attitude | position of a connection part can be kept constant. Further, the arm device may be configured such that the third link is connected to the first link via the fourth joint portion, and the second link is connected to the third link via the second joint portion. In this case, the arm device can move the gripping portion in the translation direction by rotating the first, second, and fourth joint portions.
上記実施形態において、制御装置５の制御部５５は、並進力変化量Δｆ及び旋回力変化量Δτのコンプライアント制御を行なっているが、これに限定されない。制御部５５は、並進力及び旋回トルクのうち一方のみのコンプライアント制御を行なってもよい。
In the above embodiment, the
また、本発明は、例えば、図１８に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
In addition, the present invention can realize the processing shown in FIG. 18 by causing a CPU to execute a computer program, for example.
The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。 The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.
１ 搬送ロボット、２ ロボット本体、３ 移動装置、４ アーム装置、５ 制御装置、６ 環境センサ、７ トルクセンサ、８ 角度センサ、５１ 並進力検出部、５２ 旋回力検出部、５３ 並進力変化量算出部、５４ 旋回力変化量算出部、５５ 制御部
DESCRIPTION OF
Claims (9)
前記ロボット本体に設けられ、移動する移動手段と、
前記ロボット本体に設けられ、カートを牽引し、並進方向及び旋回方向に移動可能なアームと、
前記アームに対し並進方向に作用する並進力を検出する並進力検出手段と、
前記アームに対し旋回方向に作用する旋回力を検出する旋回力検出手段と、
を備える搬送ロボットであって、
前記並進力検出手段により検出された並進力に基づいて、前記カートに対して外力が作用したときの前記アームに対する並進力の変化量を算出する並進力変化量算出手段と、
前記旋回力検出手段により検出された旋回力に基づいて、前記カートに対して外力が作用したときの前記アームに対する旋回力の変化量を算出する旋回力変化量算出手段と、
前記並進力変化量算出手段により算出された並進力の変化量及び前記旋回力変化量算出手段により算出された旋回力の変化量、のうちの少なくとも一方に倣うように倣い制御を実行する制御手段と、を備えることを特徴とする搬送ロボット。 The robot body,
A moving means provided on the robot body for moving;
An arm that is provided in the robot body, pulls the cart, and is movable in a translational direction and a turning direction;
A translation force detecting means for detecting a translation force acting in a translation direction on the arm;
A turning force detecting means for detecting a turning force acting on the arm in a turning direction;
A transfer robot comprising:
A translational force change amount calculating means for calculating a change amount of the translational force with respect to the arm when an external force is applied to the cart based on the translational force detected by the translational force detecting unit;
Based on the turning force detected by the turning force detection means, a turning force change amount calculating means for calculating a change amount of the turning force with respect to the arm when an external force is applied to the cart;
Control means for performing copying control so as to follow at least one of the change amount of the translational force calculated by the translational force change amount calculation means and the change amount of the turning force calculated by the turning force change amount calculation means And a transport robot.
前記制御手段は、前記並進力変化量算出手段により算出された並進力の変化量が第１閾値以上、及び前記旋回力変化量算出手段により算出された旋回力の変化量が第２閾値以上、のうちの少なくとも一方となるとき、前記倣い制御を変更する、ことを特徴とする搬送ロボット。 The transfer robot according to claim 1,
The control means has a translational force change amount calculated by the translational force change amount calculating unit equal to or greater than a first threshold value, and a turning force variation amount calculated by the turning force change amount calculating means is equal to or greater than a second threshold value. The transfer robot is characterized in that the copying control is changed when at least one of them is selected.
前記制御手段は、前記並進力変化量算出手段により算出された並進力の変化量が第１閾値以上、及び前記旋回力変化量算出手段により算出された旋回力の変化量が第２閾値以上、のうちの少なくとも一方となるとき、前記移動手段を制御して、前記カートを所定位置まで後退させ、前記カートの進行方向に対して、該カートに作用する外力の位置と反対側へ前記カートを所定量移動させる復帰制御を行なう、ことを特徴とする搬送ロボット。 The transfer robot according to claim 2,
The control means has a translational force change amount calculated by the translational force change amount calculating unit equal to or greater than a first threshold value, and a turning force variation amount calculated by the turning force change amount calculating means is equal to or greater than a second threshold value. The moving means is controlled to retract the cart to a predetermined position, and the cart is moved in the direction opposite to the position of the external force acting on the cart with respect to the traveling direction of the cart. A transfer robot characterized in that a return control for moving a predetermined amount is performed.
前記並進力変化量算出手段は、前記カートの各車輪の転がり抵抗に基づいて該カートに外力が作用していないときの並進力の想定値を算出し、前記並進力検出手段により検出された並進力と該算出した並進力の想定値とに基づいて前記並進力の変化量を算出し、
前記旋回力変化量算出手段は、前記カートの各車輪の転がり抵抗に基づいて該カートに外力が作用していないときの旋回力の想定値を算出し、前記旋回力検出手段により検出された旋回力と該算出した旋回力の想定値とに基づいて前記旋回力の変化量を算出する、ことを特徴とする搬送ロボット。 The transfer robot according to any one of claims 1 to 3,
The translational force change amount calculating means calculates an estimated value of translational force when no external force is applied to the cart based on the rolling resistance of each wheel of the cart, and the translational force detected by the translational force detecting means. Calculating the amount of change in the translational force based on the force and the calculated value of the translational force,
The turning force change amount calculation means calculates an assumed value of turning force when no external force is applied to the cart based on rolling resistance of each wheel of the cart, and the turning force detected by the turning force detection means. A transfer robot, characterized in that the amount of change in the turning force is calculated based on the force and the calculated assumed value of the turning force.
当該搬送ロボットが移動する環境内の領域毎に、前記転がり抵抗が設定された環境地図情報を記憶する記憶手段を更に備え、
前記並進力変化量算出手段及び旋回力変化量算出手段は、前記記憶手段の環境地図情報と、当該搬送ロボットの現在位置情報と、に基づいて、前記転がり抵抗を算出し、該算出した転がり抵抗に基づいて、前記並進力の想定値及び前記旋回力の想定値を夫々算出する、ことを特徴とする搬送ロボット。 The transfer robot according to claim 4,
For each region in the environment where the transfer robot moves, the storage robot further comprises storage means for storing environment map information in which the rolling resistance is set,
The translational force change amount calculating means and the turning force change amount calculating means calculate the rolling resistance based on the environment map information of the storage means and the current position information of the transfer robot, and calculate the rolling resistance. An estimated value of the translational force and an assumed value of the turning force are calculated based on the above, respectively.
前記制御手段は、
前記並進力変化量算出手段により算出された並進力の変化量に基づいて、前記並進力のインピーダンス制御又はアドミタンス制御を行ない、
前記旋回力変化量算出手段により算出された旋回力の変化量に基づいて、前記旋回力のインピーダンス制御又はアドミタンス制御を行なう、ことを特徴とする搬送ロボット。 The transfer robot according to claim 4 or 5,
The control means includes
Based on the translational force variation calculated by the translational force variation calculator, the impedance control or admittance control of the translational force is performed.
A transfer robot characterized in that impedance control or admittance control of the turning force is performed based on a turning force change amount calculated by the turning force change amount calculating means.
当該搬送ロボットが走行する斜面の傾斜を検出する傾斜検出手段を更に備え、
前記並進力変化量算出手段は、前記傾斜検出手段により検出された前記斜面の傾斜に基づいて補正した前記並進力の想定値を算出し、該補正した並進力の想定値に基づいて前記並進力の変化量を算出し、
前記旋回力変化量算出手段は、前記傾斜検出手段により検出された前記斜面の傾斜に基づいて補正した前記旋回力の想定値を算出し、該補正した旋回力の想定値に基づいて前記旋回力の変化量を算出する、ことを特徴とする搬送ロボット。 The transfer robot according to any one of claims 4 to 6,
Further comprising an inclination detecting means for detecting the inclination of the slope on which the transfer robot travels;
The translational force change amount calculating means calculates an estimated value of the translational force corrected based on the inclination of the slope detected by the inclination detecting means, and the translational force based on the corrected estimated value of the translational force. The amount of change in
The turning force change amount calculating means calculates an assumed value of the turning force corrected based on the inclination of the slope detected by the inclination detecting means, and the turning force is calculated based on the corrected assumed value of the turning force. A transfer robot, characterized by calculating the amount of change.
当該搬送ロボットが移動する環境内の領域毎に、前記カートの各車輪の転がり抵抗が設定された環境地図情報を記憶する記憶手段を更に備え、
前記制御手段は、前記記憶手段の環境地図情報と、当該搬送ロボットの現在位置情報と、に基づいて、前記第１及び第２閾値を変更する、ことを特徴とする搬送ロボット。 The transfer robot according to claim 2,
For each region in the environment in which the transfer robot moves, the storage robot further includes storage means for storing environment map information in which rolling resistance of each wheel of the cart is set,
The transport robot, wherein the control means changes the first and second threshold values based on environmental map information in the storage means and current position information of the transport robot.
前記ロボット本体に設けられ、移動する移動手段と、
前記ロボット本体に設けられ、カートを牽引し、並進方向及び旋回方向に移動可能なアームと、を備える搬送ロボットの制御方法であって、
前記アームに対し並進方向に作用する並進力を検出するステップと、
前記アームに対し旋回方向に作用する旋回力を検出するステップと、
前記検出された並進力に基づいて、前記カートに対して外力が作用したときの前記アームに対する並進力の変化量を算出するステップと、
前記検出された旋回力に基づいて、前記カートに対して外力が作用したときの前記アームに対する旋回力の変化量を算出するステップと、
前記算出された並進力の変化量及び前記算出された旋回力の変化量、のうちの少なくとも一方に倣うように倣い制御を実行するステップと、を含むことを特徴とする搬送ロボットの制御方法。 The robot body,
A moving means provided on the robot body for moving;
A control method for a transfer robot, provided on the robot body, tow a cart and move in a translational direction and a turning direction,
Detecting a translational force acting in a translational direction on the arm;
Detecting a turning force acting on the arm in a turning direction;
Calculating a change amount of the translational force on the arm when an external force is applied to the cart based on the detected translational force;
Calculating an amount of change in the turning force with respect to the arm when an external force is applied to the cart based on the detected turning force;
And a step of performing copying control so as to follow at least one of the calculated amount of change in translational force and the amount of change in the calculated turning force.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2014262031A JP6332018B2 (en) | 2014-12-25 | 2014-12-25 | Transfer robot and control method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2014262031A JP6332018B2 (en) | 2014-12-25 | 2014-12-25 | Transfer robot and control method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2016120561A true JP2016120561A (en) | 2016-07-07 |
| JP6332018B2 JP6332018B2 (en) | 2018-05-30 |
Family
ID=56326891
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2014262031A Active JP6332018B2 (en) | 2014-12-25 | 2014-12-25 | Transfer robot and control method thereof |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6332018B2 (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2019058956A (en) * | 2017-09-25 | 2019-04-18 | ファナック株式会社 | Robot system |
| JP2019530104A (en) * | 2016-10-09 | 2019-10-17 | 浙江国自機器人技術有限公司Ｚｈｅｊｉａｎｇ Ｇｕｏｚｉ Ｒｏｂｏｔｉｃｓ Ｃｏ．， Ｌｔｄ． | Inventory management system, transport device, and method of combining transport device with transport object |
| JP2020032488A (en) * | 2018-08-30 | 2020-03-05 | ファナック株式会社 | Human cooperative robot system |
| JP2021010992A (en) * | 2019-07-09 | 2021-02-04 | セイコーエプソン株式会社 | Mobile robot |
| JP2021071855A (en) * | 2019-10-30 | 2021-05-06 | 株式会社東芝 | Automatic guided vehicle |
| CN112947439A (en) * | 2021-02-05 | 2021-06-11 | 深圳市优必选科技股份有限公司 | Position adjusting method and device, terminal equipment and readable storage medium |
| JPWO2022085625A1 (en) * | 2020-10-19 | 2022-04-28 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05324068A (en) * | 1991-04-15 | 1993-12-07 | Toshiba Corp | Driving controller for traveling robot |
| JP2006031503A (en) * | 2004-07-20 | 2006-02-02 | Sharp Corp | Autonomous travel vehicle |
| JP2006043797A (en) * | 2004-08-02 | 2006-02-16 | Honda Motor Co Ltd | Gait generating device and control device of legged mobile robot |
| JP2006305642A (en) * | 2005-04-26 | 2006-11-09 | Kawada Kogyo Kk | Humanoid robot, and detachable type buttocks |
| JP2009045682A (en) * | 2007-08-17 | 2009-03-05 | Nagaoka Univ Of Technology | Method and device for analyzing tactile sense |
| JP2009288931A (en) * | 2008-05-28 | 2009-12-10 | Murata Mach Ltd | Autonomous traveling object and its movement control method |
| WO2010058498A1 (en) * | 2008-11-19 | 2010-05-27 | 村田機械株式会社 | Autonomous mobile device |
| JP2011232815A (en) * | 2010-04-23 | 2011-11-17 | Fujitsu Ltd | Mobile object apparatus and movement control program |
-
2014
- 2014-12-25 JP JP2014262031A patent/JP6332018B2/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05324068A (en) * | 1991-04-15 | 1993-12-07 | Toshiba Corp | Driving controller for traveling robot |
| JP2006031503A (en) * | 2004-07-20 | 2006-02-02 | Sharp Corp | Autonomous travel vehicle |
| JP2006043797A (en) * | 2004-08-02 | 2006-02-16 | Honda Motor Co Ltd | Gait generating device and control device of legged mobile robot |
| JP2006305642A (en) * | 2005-04-26 | 2006-11-09 | Kawada Kogyo Kk | Humanoid robot, and detachable type buttocks |
| JP2009045682A (en) * | 2007-08-17 | 2009-03-05 | Nagaoka Univ Of Technology | Method and device for analyzing tactile sense |
| JP2009288931A (en) * | 2008-05-28 | 2009-12-10 | Murata Mach Ltd | Autonomous traveling object and its movement control method |
| WO2010058498A1 (en) * | 2008-11-19 | 2010-05-27 | 村田機械株式会社 | Autonomous mobile device |
| JP2011232815A (en) * | 2010-04-23 | 2011-11-17 | Fujitsu Ltd | Mobile object apparatus and movement control program |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2019530104A (en) * | 2016-10-09 | 2019-10-17 | 浙江国自機器人技術有限公司Ｚｈｅｊｉａｎｇ Ｇｕｏｚｉ Ｒｏｂｏｔｉｃｓ Ｃｏ．， Ｌｔｄ． | Inventory management system, transport device, and method of combining transport device with transport object |
| JP2019058956A (en) * | 2017-09-25 | 2019-04-18 | ファナック株式会社 | Robot system |
| US10639799B2 (en) | 2017-09-25 | 2020-05-05 | Fanuc Corporation | Robot system |
| JP2020032488A (en) * | 2018-08-30 | 2020-03-05 | ファナック株式会社 | Human cooperative robot system |
| US11897135B2 (en) | 2018-08-30 | 2024-02-13 | Fanuc Corporation | Human-cooperative robot system |
| JP2021010992A (en) * | 2019-07-09 | 2021-02-04 | セイコーエプソン株式会社 | Mobile robot |
| JP2021071855A (en) * | 2019-10-30 | 2021-05-06 | 株式会社東芝 | Automatic guided vehicle |
| JP7003097B2 (en) | 2019-10-30 | 2022-01-20 | 株式会社東芝 | Automated guided vehicle |
| JPWO2022085625A1 (en) * | 2020-10-19 | 2022-04-28 | ||
| JP7367231B2 (en) | 2020-10-19 | 2023-10-23 | 株式会社Preferred Robotics | Autonomous vehicles, control devices and programs |
| CN112947439A (en) * | 2021-02-05 | 2021-06-11 | 深圳市优必选科技股份有限公司 | Position adjusting method and device, terminal equipment and readable storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6332018B2 (en) | 2018-05-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6332018B2 (en) | Transfer robot and control method thereof | |
| JP6483730B2 (en) | Method, robot and computer readable medium for obtaining natural pitch and roll | |
| JP6471702B2 (en) | Article conveying device | |
| US11850751B2 (en) | Control device, control method, and control system | |
| JP2016049616A (en) | Travel robot, operation plan method therefor and program | |
| US11040449B2 (en) | Robot control system and method of controlling a robot | |
| JP2009018380A (en) | Robot, control method of the robot, and control system of the robot | |
| JPWO2014162605A1 (en) | Moving body | |
| WO2020197800A1 (en) | Multi-body controller and robot | |
| JP2017013214A (en) | Baggage loading/unloading system and method by plurality of robots | |
| JP2010082717A (en) | Inverted pendulum type movement mechanism | |
| WO2018074117A1 (en) | Moving device and moving device management system | |
| JP2007257200A (en) | Mobile body and control method for it | |
| JP7091733B2 (en) | Position estimation system, position detection method, and program | |
| JP6468127B2 (en) | Omnidirectional moving body, control method and program thereof | |
| JP2016224654A (en) | Autonomous travel robot | |
| US9833903B1 (en) | Controlling extendable legs of a robotic device for use of a mechanical gripper | |
| JP5913001B2 (en) | MOBILE BODY CONTROL DEVICE, ITS CONTROL METHOD, AND CONTROL PROGRAM | |
| JP2016062441A (en) | Omnidirectional transfer vehicle | |
| CN111070203B (en) | Control system, control method, and control program | |
| JP6350412B2 (en) | Robot fall prevention method | |
| US20130282237A1 (en) | Inverted pendulum type moving body | |
| KR101107222B1 (en) | Mobile robot and controlling method of the same | |
| JP2018167380A (en) | Control device | |
| JP6558036B2 (en) | Autonomous mobile body and control method thereof |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20161221 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20171031 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20171031 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20171218 |
|
| 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: 20180403 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20180416 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 6332018 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |