JP4304495B2 - Route planning method - Google Patents

Route planning method Download PDF

Info

Publication number
JP4304495B2
JP4304495B2 JP2004228461A JP2004228461A JP4304495B2 JP 4304495 B2 JP4304495 B2 JP 4304495B2 JP 2004228461 A JP2004228461 A JP 2004228461A JP 2004228461 A JP2004228461 A JP 2004228461A JP 4304495 B2 JP4304495 B2 JP 4304495B2
Authority
JP
Japan
Prior art keywords
route
intermediate point
path
point
planning method
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.)
Expired - Fee Related
Application number
JP2004228461A
Other languages
Japanese (ja)
Other versions
JP2006048372A (en
Inventor
真太郎 吉澤
豊 平野
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Priority to JP2004228461A priority Critical patent/JP4304495B2/en
Publication of JP2006048372A publication Critical patent/JP2006048372A/en
Application granted granted Critical
Publication of JP4304495B2 publication Critical patent/JP4304495B2/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Description

本発明は、自動機械の全体又は一部分などの移動経路を計画する経路計画方法に関する。
The present invention relates to that route planning method plan a travel route, such as all or a portion of an automatic machine.

ロボットを移動させる際や、ロボットアームを自動操作する際には、その経路を計画する必要が生じる。下記［非特許文献１］には、確率機構を用いた経路計画について記載されている。［非特許文献１］の方法では、一つのスタート位置（初期位置：初期姿勢）に対して一つのゴール位置（最終位置：最終姿勢）が設定され、この間に一つ以上の中間点を経由する経路が設定される。このとき、中間点に基づいて経路を計画する際の基準が関節空間などの探索空間であり、中間点は一度に一つのみ設定される。   When moving the robot or automatically operating the robot arm, it is necessary to plan the route. The following [Non-Patent Document 1] describes route planning using a probability mechanism. In the method of [Non-Patent Document 1], one goal position (final position: final posture) is set for one start position (initial position: initial posture), and one or more intermediate points are passed during this time. A route is set. At this time, a reference for planning a route based on the intermediate point is a search space such as a joint space, and only one intermediate point is set at a time.

なお、関節空間などの探索空間を用いた経路設定については、下記［非特許文献２］に記載がある。［非特許文献２］には、空間内でのロボットの望ましい経路を決定する方法として、空間的・時間的な経路を関節角の関数として記述する関節空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。   Note that route setting using a search space such as a joint space is described in [Non-Patent Document 2] below. [Non-Patent Document 2] describes a joint space method that describes a spatial / temporal path as a function of a joint angle as a method for determining a desired path of a robot in space. In the joint space method, one or more intermediate points are set between the start position (initial position) and the goal position (final position) of a path, and a path is created by connecting these with a spline curve.

Ｒａｎｄｏｍｉｚｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ： Ｓ．Ｍ．ＬａＶａｌｌｅ ａｎｄ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ． ： Ｐｒｏｃ ｏｆ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， １９９９Randomized Kinodynamic Planning: S. M.M. LaValle and J.M. J. et al. Kuffner, Jr. : Proc of IEEE Int. Conf. on Robotics and Automation, 1999 ロボティクス−機構・力学・制御−（第７章）、Ｊｏｈｎ Ｊ．Ｃｒａｉｇ 著、三浦宏文・下山勲 訳、共立出版株式会社、１０９６４１号公報Robotics-Mechanism, Mechanics, Control-(Chapter 7), John J. et al. Craig, translated by Hirofumi Miura and Isao Shimoyama, Kyoritsu Publishing Co., Ltd., 109641

［非特許文献１］の方法では、ロボットアームなどの経路を計画する場合などは、最終姿勢が一つであるため、この設定された一つの最終姿勢で経路設定が不可能だった場合に経路をどのように設定するかという問題が残る。実際には、最終姿勢は複数とれる場合が多く、ある最終姿勢で経路計算不可でも、別の最終姿勢では経路計算可能な場合がある。また、［非特許文献１］の方法では、関節空間などの探索空間における距離と自動機械が動く現実の作業空間における距離とは対応していないため、作業空間で無駄な動きを生じさせる経路を生成してしまうおそれがあった。   In the method of [Non-Patent Document 1], when planning a route of a robot arm or the like, there is only one final posture. Therefore, when a route cannot be set with this one final posture set, the route The question of how to set up remains. Actually, there are many cases where a plurality of final postures can be taken, and there are cases where a route can be calculated in another final posture even if a route cannot be calculated in one final posture. Further, in the method of [Non-Patent Document 1], the distance in the search space such as the joint space does not correspond to the distance in the actual work space in which the automatic machine moves. There was a risk of generating.

さらに、経路生成途中で設定される中間点はランダムに一つずつ生成されるため、運動学的に望ましくない中間点への無駄な経路計算が行われる場合があった。従って、本発明の目的は、運動学的及び動力学的に望ましく、動作移動の効率を考慮した経路を計画することが可能な経路計画方法を提供することにある。   Furthermore, since the intermediate points set during the route generation are generated one by one at random, there is a case where a wasteful route calculation to a kinematically undesirable intermediate point is performed. Accordingly, an object of the present invention is to provide a route planning method that can plan a route that is desirable in terms of kinematics and dynamics and that takes into consideration the efficiency of motion movement.

請求項１に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成させ、中間点を用いて初期位置から最終位置までの間に経路を計画するものであり、一つの初期位置に対し複数の最終位置を予め設定し、中間位置を介して初期位置と各最終位置とを繋ぐ複数の候補となる経路を生成することを特徴としている。 In the route planning method according to claim 1, an intermediate point is generated probabilistically between an initial position and a final position, and a route is planned between the initial position and the final position using the intermediate point. A plurality of final positions are set in advance for one initial position, and a plurality of candidate paths that connect the initial position and each final position via an intermediate position are generated .

請求項２に記載の発明は、請求項１に記載の経路計画方法において、初期位置及び前記最終位置の双方からそれぞれ経路を生成するもので、最終位置からの経路は、（１）少なくとも一つの中間点を生成し、（２）生成された中間点に基づいて複数の最終位置から一つを選択し、（３）中間点と選択された最終位置との間に経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴としている。
The invention described in claim 2, in route planning method according to claim 1, intended to generate respective paths from both the initial position and the final position, the path from the end position, (1) at least one Generate two intermediate points, (2) select one of a plurality of final positions based on the generated intermediate points, and (3) at least part of the path between the intermediate points and the selected final positions. It is generated by performing the generation at least once.

請求項３に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に、自動機械の全体又は一部分の経路を計画するもので、自動機械が存在する実空間上に作業空間を定義すると共に、この作業空間における自動機械の状態が一点として表される多次元座標空間を定義し、作業空間上の初期位置と最終位置との間の経路設定時に、多次元座標空間上における初期位置に対応する点と最終位置に対応する点とを結ぶ経路を、作業空間における距離と多次元座標空間における距離とを用いて生成することを特徴としている。
 ROUTE planning method according to claim 3, the midpoint between the initial and final positions stochastically generated, during a period from the initial position by using the intermediate point to the final position, the entire automatic machine or This is a plan for a part of the route. In addition to defining the work space on the real space where the automatic machine exists, the multi-dimensional coordinate space where the state of the automatic machine in this work space is expressed as one point is defined. When setting the path between the initial position and the final position, the path connecting the point corresponding to the initial position in the multidimensional coordinate space and the point corresponding to the final position is the distance in the work space and the distance in the multidimensional coordinate space. It is generated using and.

請求項４に記載の発明は、請求項３に記載の経路計画方法において、作業空間における距離を用いて中間点の設定を行い、この中間点に向けた経路の生成を多次元座標空間における距離を用いて決定することを特徴としている。
According to a fourth aspect of the invention, the route planning method according to claim 3, to set the intermediate point using the distance in the work space, the generation of the path towards the midpoint of the multi-dimensional coordinate space It is characterized by determining using distance.

請求項５に記載の発明は、請求項４に記載の経路計画方法において、経路は、（１）少なくとも一つの中間点を多次元座標空間上に生成し、（２）多次元座標空間上における初期位置又は最終位置あるいは既に生成されている経路上の点の中から、生成された中間点と作業空間上で最も近い距離にある点を経路点として選択し、（３）多次元座標空間における距離を用いて、経路点から中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴としている。
Invention according to claim 5, in route planning method according to claim 4, pathway, (1) at least one intermediate points produced on multi-dimensional coordinate space, (2) multi-dimensional coordinate space From the initial position, the final position, or the points on the route that have already been generated, a point closest to the generated intermediate point on the work space is selected as a route point, and (3) a multidimensional coordinate space Using the distance at, the collision determination between the obstacle and the route being generated between the route point and the intermediate point is performed, and (4) the next route point is set at least once when there is no collision. It is generated by performing the above.

請求項６に記載の経路計画方法は、初期位置と最終位置との間に中間点を確率的に生成し、中間点を用いて初期位置から最終位置までの間に経路を計画するもので、中間点の候補となる中間点候補が同時に複数生成され、複数の中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴としている。
The route planning method according to claim 6, wherein an intermediate point is generated probabilistically between an initial position and a final position, and a route is planned between the initial position and the final position using the intermediate point. A plurality of intermediate point candidates that are intermediate point candidates are generated simultaneously, and one intermediate point is selected from a plurality of intermediate point candidates based on a predetermined selection criterion.

請求項７に記載の発明は、請求項６に記載の経路計画方法において、選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴としている。
The invention described in claim 7 is the route planning method according to claim 6, selection criteria, and wherein a is based on kinematic Manipulability or Kinetic Manipulability.

請求項１に記載の経路計画方法によれば、経路の最終位置を予め複数設定し、これらに対して経路計算を行うことで、取りうる経路が多くなるためより適切な経路を設定しやすくなる。この結果、より効率よく、無駄のない経路を計画することができる。また、請求項２に記載の経路計画方法によれば、予め設定されたすべての最終位置に対して経路の全行程を計算する場合よりも少ない計算量で経路を計画することができる。
According to route planning method according to claim 1, in advance a plurality sets the final position of the path, by performing these with respect to the path calculation, easy to set the appropriate route than for can take paths increases Become. As a result, a more efficient and lean route can be planned. Further, according to the route planning method according to claim 2, it is possible to plan a route with a small amount of calculation than when calculating the total stroke of the paths for preset all final positions.

請求項３に記載の経路計画方法によれば、自動装置（自動装置自体の移動経路やロボットアームの移動経路など）の状態（姿勢など）を多次元座標空間上の一点で表し、実際の作業空間と多次元座標空間とのそれぞれの空間上での距離とを使って自動装置の経路を計画する。このように、実際の作業空間における距離と多次元座標空間における距離とをそれぞれ適切に使い分けることで、動作移動の効率を考慮した無駄のない経路を計画することができる。
According to route planning method according to claim 3, represents the state of the automatic apparatus (such as a moving path of the moving path or a robot arm of an automatic device itself) (such as posture) at one point on the multidimensional coordinate space, the actual The path of the automatic device is planned using the distance between the work space and the multidimensional coordinate space on each space. As described above, by appropriately using the distance in the actual work space and the distance in the multi-dimensional coordinate space, it is possible to plan a lean route in consideration of the efficiency of motion movement.

また、請求項４に記載の経路計画方法によれば、自動機械の経路上に位置することとなる中間点の設定には実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いることで演算負荷を軽減することができる。
Further, according to claim 4, according to the route planning method, by using the distance of the actual workspace to set the intermediate point to be positioned in the path of the automatic machine, more efficiently, the waste of No route can be planned. Further, in the process of sequentially extending the route toward the set intermediate point, the calculation load can be reduced by using the distance in the multidimensional coordinate space.

また、請求項５に記載の経路計画方法によれば、中間点との間に経路を計画する経路点（又は、初期位置／最終位置）の選択には、実際の作業空間の距離を用いることで、より効率よく、無駄のない経路を計画することができる。また、設定された中間点に向けて経路を順次伸ばして行く過程では多次元座標空間における距離を用いて衝突判定（作業空間上で自動機械が障害物と衝突するか否かの判定）を行うことで演算負荷を軽減することができる。
Further, according to the route planning method according to claim 5, path points to plan a path between the intermediate point (or the initial position / end position) in the selection of, using the distance of the actual workspace Thus, a more efficient and lean route can be planned. In addition, in the process of sequentially extending the route toward the set intermediate point, collision determination (determination of whether or not the automatic machine collides with an obstacle on the work space) is performed using the distance in the multidimensional coordinate space. Thus, the calculation load can be reduced.

請求項６に記載の経路計画方法によれば、中間点となる候補が複数生成され、この中から所定の選定基準に基づいて中間点が決定される。このため、運動学的に望ましく、無駄のない経路を計画することができる。また、請求項７に記載の経路計画方法によれば、選択基準が自動装置の操作性を考慮した可操作性（運動学的又は動力学的）に基づくものであるため、より一層、運動学的及び動力学的に望ましい経路を計画することができる。
According to route planning method according to claim 6, candidate having the midpoint is more produced, the intermediate point is determined on the basis of a predetermined selection criterion from among them. For this reason, it is possible to plan a kinematically desirable and lean route. Further, according to according to the route planning method according to claim 7, since the selection criterion is based on the manipulability Considering the operation of the automatic device (kinematic or kinetic), more exercise It is possible to plan pathologically and kinetically desirable paths.

本発明の経路計画方法の一実施形態について以下に説明する。まず、本実施形態の経路計画方法を演算する装置（経路生成手段）の構成を図１に示す。経路生成手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、経路生成手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。
An embodiment of the route planning method of the present invention will be described below. First, a structure of an apparatus for calculating a route planning method of the present embodiment (route generating means) shown in FIG. The route generation means 1 is specifically configured by a computer. As shown in FIG. 1, the route generation means 1 is an external environment information input means 2 that is a means for inputting external environment information, and a route that outputs a route that is in the process of generation or a route that is finally generated (set). Output means 3.

外部環境情報入力手段２は、移動経路を生成する自動機械（ロボット自体やロボットアームなど）周辺の情報、特に障害物情報を外部環境として取得する手段であり、カメラや各種センサなどの情報取得デバイスが例として挙げられる。あるいは、外部環境情報入力手段２は、キーボードや光ディスクドライブなどであっても良く、オペレータが手動で外部環境情報を入力しても良いし、すでにデータ化されて光ディスクに格納されている外部環境情報を入力しても良い。経路出力手段３は、具体的には、モニタやプリンタ、記憶可能な媒体を扱うドライブなどである。   The external environment information input means 2 is a means for acquiring information around an automatic machine (such as a robot itself or a robot arm) that generates a movement route, particularly obstacle information as an external environment, and is an information acquisition device such as a camera or various sensors. Is given as an example. Alternatively, the external environment information input means 2 may be a keyboard, an optical disk drive, etc., and the operator may input the external environment information manually, or the external environment information already converted into data and stored on the optical disk May be entered. The route output means 3 is specifically a monitor, a printer, a drive for handling a storable medium, or the like.

外部環境情報入力手段２から入力された外部環境情報は、環境情報記憶手段４に蓄積される。環境情報記憶手段４は、具体的にはハードディスクやＲＡＭなどである。環境情報記憶手段４には、知識としての環境情報（すでに知識として保持している情報）と、上述したような、外部環境情報入力手段２によって新たに入力された環境情報とを保持する。外部環境情報入力手段２によって新たに入力された環境情報は、その後は知識としての環境情報として蓄積される。経路生成手段１は、経路のスタート位置（初期位置）とゴール位置（最終位置）とを最終的に決定するスタート・ゴール位置決定手段５や、スタート位置とゴール位置との間に中間点を設定（仮設定）する中間点生成手段６なども有している。中間点生成手段６などは、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。   The external environment information input from the external environment information input unit 2 is accumulated in the environment information storage unit 4. Specifically, the environment information storage unit 4 is a hard disk, a RAM, or the like. The environment information storage means 4 holds environment information as knowledge (information already held as knowledge) and the environment information newly input by the external environment information input means 2 as described above. The environment information newly input by the external environment information input means 2 is then stored as environment information as knowledge. The route generation unit 1 sets a start / goal position determination unit 5 that finally determines a start position (initial position) and a goal position (final position) of the route, and an intermediate point between the start position and the goal position. It also has intermediate point generation means 6 (temporary setting). The intermediate point generation means 6 and the like are realized by a program stored in a hard disk or a ROM and a CPU that executes the program.

次に、経路生成の手順について、図２のフローチャートを参照しつつつ、順を追って説明する。ここでは、自動機械がロボットアームである場合を例にして説明する。図３は、ロボットアーム１００を模式的に示した図である。このロボットアーム１００は、図３に示されるように、複数のリンク（線状の部材）１０１と、二つのリンク１０１の間に配置された関節ジョイント１０２とからなっている。各関節ジョイント１０２は一自由度の関節である。また、アーム１００の先端には、把持部１０３が取り付けられている。ここでは、把持部１０３の根本、即ち、最も末端側のリンク１０１の先端部分（手首位置Ｘ）の移動軌跡を生成する。   Next, the route generation procedure will be described step by step with reference to the flowchart of FIG. Here, a case where the automatic machine is a robot arm will be described as an example. FIG. 3 is a diagram schematically showing the robot arm 100. As shown in FIG. 3, the robot arm 100 includes a plurality of links (linear members) 101 and a joint joint 102 disposed between the two links 101. Each joint joint 102 is a joint with one degree of freedom. In addition, a grip portion 103 is attached to the tip of the arm 100. Here, the movement trajectory of the root of the grip portion 103, that is, the tip end portion (the wrist position X) of the most distal link 101 is generated.

実際には各関節ジョイント１０２にアクチュエータが内蔵されており、その関節ジョイント１０２に接合している一対のリンク１０１によって形成される角度を変更することができる。なお、ここでは、根本側のリンク１０１の基端部と、末端側リンク１０１の把持部１０３との接合部（手首位置Ｘ）はリジッドであるものとして説明する。なお、ロボットアーム１００が実存している空間を、以下、作業空間と言うこととする。   Actually, an actuator is built in each joint joint 102, and an angle formed by a pair of links 101 joined to the joint joint 102 can be changed. In the following description, it is assumed that the joint portion (wrist position X) between the proximal end portion of the link 101 on the root side and the grip portion 103 of the distal side link 101 is rigid. The space where the robot arm 100 exists is hereinafter referred to as a work space.

また、ロボットアーム１００の状態、即ち、ロボットアーム１００がどのような姿勢であるかは、各関節ジョイント１０２の角度が分かれば把握することができる。今ここで、図３中の三つの各関節ジョイント１０２での角度θ１，θ２，θ３それぞれ座標軸として持つ多次元座標空間（この例では三次元座標空間）を設定すれば、ロボットアーム１００の姿勢は、この多次元座標空間上の一点［座標（θ１，θ２，θ３）］によって表すことができる。 Further, the state of the robot arm 100, that is, the posture of the robot arm 100 can be grasped if the angles of the joint joints 102 are known. Now, where the angle theta 1 at each of the three articulation joint 102 in FIG. 3, theta 2, (in this example three-dimensional coordinate space) multidimensional coordinate space having coordinate axes theta 3 respectively be set, the robot arm 100 Can be represented by one point [coordinates (θ 1 , θ 2 , θ 3 )] on the multidimensional coordinate space.

作業空間に対してｘ，ｙ座標軸を設定した場合、ロボットアーム１００の姿勢を表す手首位置Ｘの座標（ｘ，ｙ）は、多次元座標空間上の座標（θ１，θ２，θ３）を変換することで容易に得ることができる。なお、ここでは、ロボットアーム１００の状態（姿勢）を三次元座標で表したが、二次元や、より多次元の座標空間上で表すこともできる。例えば、上述した関節ジョイント１０２が七つあるようなロボットアームであれば、七次元座標空間上の一点としてその状態（姿勢）を表すことができる。あるいは、三つの関節ジョイントと一つの伸縮部とを持つようなロボットアームであれば、四次元座標空間上の一点としてその状態（姿勢）を表すことができる。 When x and y coordinate axes are set for the work space, the coordinates (x, y) of the wrist position X representing the posture of the robot arm 100 are coordinates (θ 1 , θ 2 , θ 3 ) in the multidimensional coordinate space. Can be easily obtained by converting. Here, the state (posture) of the robot arm 100 is represented by three-dimensional coordinates, but may be represented by a two-dimensional or multi-dimensional coordinate space. For example, if the robot arm has seven joint joints 102 described above, the state (posture) can be expressed as one point on the seven-dimensional coordinate space. Or if it is a robot arm which has three joint joints and one expansion-contraction part, the state (posture) can be represented as one point on four-dimensional coordinate space.

経路生成に際しては、まず、外部環境情報入力手段２によって外部環境情報が入力される。すでに入力された環境情報記憶手段４に蓄積された情報のみで不足がない場合は、新たな外部環境情報の入力は必ずしも必要ない。次に、経路のスタート位置とゴール位置とを上述した作業空間上にオペレータがセットする。これらの位置があらかじめプリセットされているのであれば、オペレータによる入力は必要ない。入力は、キーボードから座標位置を入力するなどして設定することができる。あるいは、経路生成手段１にロボットアーム１００を接続し、ロボットアーム１００を実際にスタート・ゴール位置に位置させたときの座標などを経路生成手段１に読み込ませるようにしても良い。また、装置に組み込まれたカメラや各種センサによって外部環境情報が自動又は半自動で取得されるようにしても良い。   When generating a route, first, external environment information is input by the external environment information input means 2. If there is no shortage of information that has already been input and stored in the environment information storage means 4, it is not always necessary to input new external environment information. Next, the operator sets the start position and goal position of the route on the above-described work space. If these positions are preset, no operator input is required. The input can be set by inputting a coordinate position from the keyboard. Alternatively, the robot arm 100 may be connected to the route generation unit 1 so that the route generation unit 1 reads the coordinates when the robot arm 100 is actually positioned at the start / goal position. The external environment information may be acquired automatically or semi-automatically by a camera or various sensors incorporated in the apparatus.

このとき、ゴール位置は複数設定される。例えば、ここではロボットアーム１００について説明しているが、最終位置で対象物を把持するとした場合に、対象物を把持する際のロボットアーム１００の姿勢（手首位置Ｘ）はいくつか考えられる。対象物を上方からつかんでも良いし、右／左側方からつかんでもよい。このようなバリエーションを考えて、ゴール位置を予め複数設定する。ロボットアーム以外の自動機械の場合であっても、ゴール位置の取り方にバリエーションを持たせられる場合が多いので、このような場合もゴール位置を複数設定する。   At this time, a plurality of goal positions are set. For example, although the robot arm 100 has been described here, there are several possible postures (wrist positions X) of the robot arm 100 when gripping the target object when the target object is gripped at the final position. The object may be grasped from above or from the right / left side. Considering such variations, a plurality of goal positions are set in advance. Even in the case of an automatic machine other than the robot arm, there are many cases where variations are made in the way of obtaining the goal position. In such a case, a plurality of goal positions are set.

スタート・ゴール位置決定手段５は、環境情報記憶手段４に蓄積された情報と照らし合わせて、入力されたスタート・ゴール位置が障害物と干渉していないか否かを判断し、干渉していない場合はこれらをスタート・ゴール位置として決定する（ステップ３００）。なお、干渉の有無の検証については、追って詳しく説明する。もし、入力されたスタート・ゴール位置が障害物と干渉している場合は、干渉しているゴール位置については削除するなどすると共に、必要であればオペレータに対して再入力を促し、再入力されたスタート・ゴール位置について同様の判定を行う。この手順は、スタート・ゴール位置が決定されるまで反復される。   The start / goal position determining means 5 determines whether or not the input start / goal position interferes with an obstacle in comparison with the information stored in the environmental information storage means 4 and does not interfere. In this case, these are determined as start / goal positions (step 300). The verification of the presence / absence of interference will be described in detail later. If the entered start / goal position interferes with an obstacle, the interfering goal position is deleted, and if necessary, the operator is prompted to re-enter and re-entered. The same determination is made for the start / goal position. This procedure is repeated until the start / goal position is determined.

決定された作業空間上のスタート・ゴール位置は、多次元座標空間上に変換される。なお、ロボットアーム１００で対象物を把持する際には、つかむ方向の違いによる手首位置Ｘの違いによって、ロボットアーム１００のアームの姿勢にはバリエーションが生まれる。また、ゴール位置の手首位置Ｘが一つ決まったときであっても、そのときのロボットアーム１００のアームの姿勢（各関節角度の組み合わせ）にはバリエーションが生まれる。ここでは、これらのバリエーションも含めて、多次元座標空間上に複数のゴール位置が設定される。このため、多次元座標空間上には複数のゴール位置が設定される。ここでは、説明の便宜上、多次元座標空間を二次元座標空間θｘ−θｙ（関節ジョイント１０２が二つのロボットアームの場合に相当）として説明する。図４に示されるように、ここでは、二次元座標空間θｘ−θｙ上に一つのスタート位置Ｓと複数のゴール位置Ｇとが設定される。なお、スタート・ゴール位置は、はじめから多次元座標空間上の座標として入力されても構わない。 The determined start / goal position on the work space is converted into a multidimensional coordinate space. Note that when the object is gripped by the robot arm 100, the posture of the arm of the robot arm 100 varies depending on the difference in the wrist position X due to the difference in the gripping direction. Even when one wrist position X of the goal position is determined, variations occur in the posture of the robot arm 100 at that time (combination of each joint angle). Here, including these variations, a plurality of goal positions are set on the multidimensional coordinate space. For this reason, a plurality of goal positions are set on the multidimensional coordinate space. Here, for convenience of explanation, the multidimensional coordinate space will be described as a two-dimensional coordinate space θ x −θ y (corresponding to the case where the joint joint 102 has two robot arms). As shown in FIG. 4, here, one start position S and a plurality of goal positions G are set on the two-dimensional coordinate space θ x -θ y . Note that the start / goal position may be input from the beginning as coordinates in a multidimensional coordinate space.

次に、確率機構を用いた公知の手法（上記［非特許文献１］などを参照）によって、多次元座標空間上で、スタート・ゴール位置の間に複数の中間点が仮設定（複数の中間点候補ｍ１が設定）される（中間点候補設定行程：ステップ３０５）。複数の中間点候補ｍ１を上述した二次元座標空間θｘ−θｙに設定した状態を図５に例示する。この中間点候補ｍ１の設定（中間点の仮設定）は、中間点生成手段６によって行われる。図５に示されるように、複数の中間点候補ｍ１が設定されたら、次に、この設定された中間点候補ｍ１の中から、経路設定に利用するために好適なものを中間点Ｍ１として設定する（中間点決定行程）。 Next, a plurality of intermediate points are provisionally set between the start and goal positions (multiple intermediate points) in a multidimensional coordinate space by a known method using a probability mechanism (see the above [Non-Patent Document 1] etc.). point candidate m 1 is set) by the (midpoint candidate setting process: step 305). FIG. 5 illustrates a state where a plurality of intermediate point candidates m 1 are set in the above-described two-dimensional coordinate space θ x −θ y . The setting of the intermediate point candidate m 1 (temporary setting of the intermediate point) is performed by the intermediate point generating means 6. As shown in FIG. 5, when a plurality of intermediate point candidates m 1 are set, the intermediate point M 1 that is suitable for use in path setting is next selected from the set intermediate point candidates m 1. Set as 1 (middle point determination process).

上述したように、複数の中間点候補ｍ１の中から好適なものを中間点Ｍ１として設定するが、ここでは中間点候補ｍ１のすべてに関して可操作度を算出し、可操作度の最も高い、即ち、可操作性が最も良い中間点候補ｍ１を中間点Ｍ１として採用する（ステップ３１０）。二次元座標空間θｘ−θｙにおいて、複数の中間点候補ｍ１の中から一つの中間点Ｍ１を設定した状態を図６に示す。なお、可操作性については、「コンピュータ制御機械システムシリーズ１０ ロボット制御基礎理論」（第４章） 吉川恒夫著 コロナ社 に記載されている。可操作性とは、自動装置（ここではロボットアーム１００の手先部分）をどの程度自由に操作できるかということであり、この操作能力を表すものが可操作度である。可操作性には、運動学的可操作性と動力学的可操作性とがあるが、ここでは何れの可操作性に基づいても良い。 As described above, a suitable one of the plurality of intermediate point candidates m 1 is set as the intermediate point M 1. Here, the operability is calculated for all of the intermediate point candidates m 1 , and the highest operability is obtained. The intermediate point candidate m 1 that is high, that is, has the best operability, is adopted as the intermediate point M 1 (step 310). FIG. 6 shows a state where one intermediate point M 1 is set from among a plurality of intermediate point candidates m 1 in the two-dimensional coordinate space θ x −θ y . The operability is described in “Computer Control Machine System Series 10 Basic Robot Control Theory” (Chapter 4) by Tsuneo Yoshikawa Corona. The manipulability means how freely the automatic device (here, the hand portion of the robot arm 100) can be manipulated, and the degree of manipulability represents this maneuverability. The maneuverability includes kinematic maneuverability and dynamic maneuverability, and here, any maneuverability may be used.

複数の中間点候補ｍ１から最も可操作性の良いものを中間点Ｍ１として採用した後は、作業空間上での複数のゴール位置の中から、中間点Ｍ１に対応する作業空間上の点と最も近い距離（作業空間上での距離）にあるものを選択する（ステップ３１５，３２０）。上述したように、二次元座標空間θｘ−θｙ上の中間点Ｍ１は、実際の作業空間上にある手首位置Ｘを定義し、また、上述した二次元座標空間θｘ−θｙ上の複数のゴール位置Ｇも、実際の作業空間上にある手首位置Ｘを定義する。ここでは、作業空間上において、中間点Ｍ１に最も近いゴール位置Ｇの定める手首位置Ｘを経路生成に利用するものとして選択する（説明の便宜上、選択されたゴール位置ＧをＧ１と呼ぶこととする）。なお、選択されなかったゴール位置Ｇは、後工程で利用する可能性があるため保持される。図７に上述した手順で一つのゴール位置Ｇ１が二次元座標空間θｘ−θｙ上に設定された状態を示す。 After adopting the most maneuverable one from the plurality of intermediate point candidates m 1 as the intermediate point M 1 , the plurality of goal positions on the work space are selected on the work space corresponding to the intermediate point M 1 . The one closest to the point (distance on the work space) is selected (steps 315 and 320). As described above, the intermediate point M 1 on the two-dimensional coordinate space θ x −θ y defines the wrist position X on the actual work space, and also on the two-dimensional coordinate space θ x −θ y described above. The plurality of goal positions G also define the wrist position X on the actual work space. Here, in the work space is selected as utilizing the wrist position X to the provisions of the nearest goal position G to an intermediate point M 1 to the route generation (for convenience of explanation, it is called a selected goal position G and G 1 And). The goal position G that has not been selected is held because it may be used in a later process. One goal position G 1 in the procedure described above in FIG. 7 shows a set state on a two-dimensional coordinate space θ x -θ y.

これで、スタート位置Ｓ・中間点Ｍ１・ゴール位置Ｇ１が一つずつ決定／選択されたこととなるので、スタート位置Ｓ−中間点Ｍ１と中間点Ｍ１−ゴール位置Ｇ１とを二次元座標空間θｘ−θｙ上で最短距離（関節ジョイント１０２に関する二次元座標空間θｘ−θｙでの距離）で結ぶ。なお、作業空間及び多次元座標空間のそれぞれの距離は、距離を定義する距離関数であるならば何でもよく、例えば、ユークリッド距離関数などでもよい。図８ではそれぞれ線分で表されている。そして、まず、二次元座標空間θｘ−θｙ上で、ゴール位置Ｇ１側の線分（ベクトル）についてゴール位置Ｇ１から所定長さ分が切り取られ、この所定長さ分に関して衝突判定が行われる（ステップ３２５）。衝突判定は、生成する経路が作業空間上で障害物と干渉しないかどうかを判定するものである。 Now that the start position S · midpoint M 1 · goal position G 1 is the fact that one by one determined / selected, the start position S- midpoint M 1 and the intermediate point M 1 - a goal position G 1 The two-dimensional coordinate space θ x −θ y is connected with the shortest distance (distance in the two-dimensional coordinate space θ x −θ y related to the joint joint 102). The distance between the work space and the multidimensional coordinate space may be any distance function that defines the distance, and may be, for example, a Euclidean distance function. In FIG. 8, each is represented by a line segment. First, on the two-dimensional coordinate space θ x −θ y , a predetermined length is cut from the goal position G 1 for the line segment (vector) on the goal position G 1 side, and the collision determination is performed for the predetermined length. Performed (step 325). The collision determination is to determine whether the generated route does not interfere with an obstacle on the work space.

図８に、ゴール位置Ｇ１側及びスタート位置Ｓ側の衝突判定が終了した時点での二次元座標空間θｘ−θｙを示す。衝突判定は、切り取られた所定長さ分の線分（ベクトル）をさらに微小な区間に分割し、その微小区間の境界部毎に行われる。この微小区間の判定は、ゴール位置Ｇ１側から中間点Ｍ１に向けて順次行われる。なお、ここでの「所定長さ」や「微小区間」は、二次元座標空間θｘ−θｙ上の距離（ユークリッド距離）に基づいている。このようにすることで、演算負荷を軽減することができる。一方、上述したように、複数のゴール位置Ｇから最も近い距離のゴール位置を選択するときは実際の作業空間における距離に基づいている。これは、生成される経路を無駄のないものとするためである。 FIG. 8 shows a two-dimensional coordinate space θ x −θ y at the time when the collision determination on the goal position G 1 side and the start position S side is completed. The collision determination is performed for each boundary portion of the minute section by further dividing the segment (vector) of the cut-out predetermined length into smaller sections. Determination of the small section is sequentially performed toward the goal position G 1 side midpoint M 1. Here, the “predetermined length” and the “minute section” are based on a distance (Euclidean distance) on the two-dimensional coordinate space θ x −θ y . By doing so, the calculation load can be reduced. On the other hand, as described above, when selecting the goal position closest to the plurality of goal positions G, it is based on the distance in the actual work space. This is to make the generated route useless.

このように、作業空間における距離と二次元座標空間θｘ−θｙにおける距離とを、必要に応じて使い分けることで、無駄のない経路生成と演算負荷の低減とを両立させている。ここで、実際の衝突判定についてであるが、上述した微小区間の境界部の二次元座標空間θｘ−θｙ上の位置座標が実際の作業空間に変換され、予め入力されている障害物の情報と比較され、作業空間上で干渉の有無が判定される。干渉がなければ、上述した所定長さ分の端部まで経路が生成される（ステップ３３０）。ゴール位置Ｇ１側の所定長さ分に関する衝突判定が済んだら、同様にスタート位置Ｓ側の衝突判定が行われる（ステップ３３５，３４０，３４５）。スタート位置Ｓ側に関しても干渉がなければ、上述した所定長さ分の端部まで経路が生成される（ステップ３５０）。 As described above, the distance in the work space and the distance in the two-dimensional coordinate space θ x −θ y are properly used as necessary, so that both a wasteful route generation and a reduction in calculation load are achieved. Here, as for the actual collision determination, the position coordinates on the two-dimensional coordinate space θ x −θ y of the boundary of the minute section described above are converted into the actual work space, and the obstacles inputted in advance are It is compared with the information to determine the presence or absence of interference on the work space. If there is no interference, a path is generated up to the end of the predetermined length described above (step 330). Once you have collision determination for a given length of the goal position G 1 side, the collision determination likewise start position S side is performed (step 335,340,345). If there is no interference on the start position S side, a route is generated up to the end portion of the predetermined length described above (step 350).

ゴール位置Ｇ１側及びスタート位置Ｓ側の各所定長さ分に関して衝突がなければ、中間点Ｍ１をそのままにして、さらに所定長さ分進んで衝突判定を行う（ステップ３５５−ｎｏ）。一方、このように衝突判定を進めつつ、経路を生成していくと、生成中の経路が作業空間上で障害物と衝突すると判定される場合が生じる。このように、衝突判定中にその所定長さ分について衝突があると判定された場合は、その所定長さ分（本実施形態では、スタート位置Ｓ側とゴール位置Ｇｉとの双方）については破棄され、新たな中間点Ｍｉが設定され、それ以降はこの新たな中間点Ｍｉを利用して同様の経路生成が続行される（ステップ３２５，３４５−ｎｏ）。 Without collision for each predetermined length of the goal position G 1 side and the start position S side, and the intermediate point M 1 as it performs collision determination further advances a predetermined length amount (step 355-no). On the other hand, when a route is generated while proceeding with collision determination in this way, it may be determined that the currently generated route collides with an obstacle on the work space. Thus, if it is determined that there is a conflict for the predetermined length component during a collision determination (in this embodiment, both the start position S side and goal position G i) the predetermined length fraction for Discarded, a new intermediate point M i is set, and thereafter, similar route generation is continued using this new intermediate point M i (steps 325, 345-no).

以下、中間点Ｍ１による二回目の所定長さについての経路生成中に衝突判定によって新たな中間点Ｍ２を設定する必要が生じた場合を例に説明する。新たな中間点Ｍ２を設定する場合、まず、図９に示されるように、複数の中間点候補ｍ２を二次元座標空間θｘ−θｙ上にランダムに設定する（ステップ３０５）。そして、図１０に示されるように、上述した中間点候補ｍ１の場合と同様に、中間点候補ｍ２のすべてに関して可操作度を算出し、可操作度の最も高い、即ち、可操作性が最も良い中間点候補ｍ２を中間点Ｍ２として採用する（ステップ３１０）。 Hereinafter, the case where the need to set a new intermediate point M 2 caused by the collision determination by the intermediate point M 1 in the path generation for a second time having a predetermined length as an example. When setting a new intermediate point M 2 , first, as shown in FIG. 9, a plurality of intermediate point candidates m 2 are randomly set on the two-dimensional coordinate space θ x −θ y (step 305). Then, as shown in FIG. 10, as in the case of the intermediate point candidate m 1 described above, the operability is calculated for all of the intermediate point candidates m 2 , and the operability is the highest, that is, the operability. There adopting best intermediate point candidate m 2 as the midpoint M 2 (step 310).

図１０において、複数の中間点候補ｍ２から最も可操作性の良いものを中間点Ｍ２として採用した後は、上述したゴール位置Ｇ１・このゴール位置Ｇ１から生成された経路上の点Ｇ１’（ここではこれ一つであるが他にもあれば複数の経路点のすべて）・採用されていなかった残りのゴール位置Ｇの中から、作業空間上での中間点Ｍ２に対して最も近い距離にあるものを経路生成に利用するものとして選択する（説明の便宜上、選択されたゴール位置ＧをＧ２と呼ぶこととする）（ステップ３１５，３２０）。なお、ここでは、先ほど生成したゴール位置Ｇ１からの経路は破棄されずに残される。さらに中間点Ｍｉを設定する際に、ゴール位置Ｇ１や経路点Ｇ１’が再度利用されることがあり得るからである。しかし、最終的に、ゴール位置Ｇ１からの経路が利用されずに終わる可能性もある。ただし、この場合は、作業空間上で中間点Ｍｉから遠いからであり、最終的に採用される経路は無駄のないものとなるので構わない。また、一度も選択されていないゴール位置Ｇも後工程で利用する可能性があるため保持される。 In FIG. 10, after adopting the most maneuverable one among the plurality of intermediate point candidates m 2 as the intermediate point M 2 , the above-described goal position G 1 and points on the path generated from the goal position G 1 are described. G 1 ′ (here, one but all of a plurality of route points if there are others) ・ From the remaining goal position G that has not been adopted, with respect to the intermediate point M 2 on the work space selecting certain ones closest distance as utilizing the route generation Te (for convenience of explanation, it will be referred to selected goal position G and G 2) (step 315, 320). Here, the path from the goal position G 1 generated earlier is left without being discarded. Furthermore, when setting the intermediate point M i , the goal position G 1 and the route point G 1 ′ may be used again. However, finally, the path from the goal position G 1 is possibly end up not being used. However, in this case, it is because it is far from the intermediate point M i on the work space, and the route finally adopted may not be wasted. Also, the goal position G that has never been selected is retained because it may be used in a later process.

なお、ここで中間点Ｍ２を用いて経路計画を進める際のスタート側に関しても、当初のスタート位置Ｓやこのスタート位置Ｓから既に経路設定された点（ノード）Ｓ’のうち、中間点Ｍ２からの距離が最も近いもの（ここでは点Ｓ’）から経路計画が行われる（ステップ３４０）。そしてその後も、図１１に示されるように同様な手順を繰り返し、最終的にスタート位置Ｓ側の経路とゴール位置Ｇ側の経路とが合流し、経路全体が決定される（ステップ３５５−ｙｅｓ）。このようにして生成された経路は、省略できる経路点を省略して経路の単純化を図る平坦化や、経路が直線の組み合わせでなく滑らかなものとなるように処理する平滑化が行われ、最終的に決定される（平坦化及び平滑化も、ここでは多次元座標空間上で行われる）。これらの平坦化や平滑化には公知の方法を用いることができる。このため、ここでは、これらの平坦化や平滑化についての詳しい説明は省略する。 Here, with regard start side when advancing the route planning by using an intermediate point M 2, among the points (nodes) S ', which has already been routed from the beginning of the start position S and the start position S, the midpoint M The route plan is performed from the closest distance from 2 (here, the point S ′) (step 340). After that, the same procedure is repeated as shown in FIG. 11, and the route on the start position S side and the route on the goal position G side finally join to determine the entire route (step 355-yes). . The route generated in this way is subjected to flattening that simplifies the route by omitting route points that can be omitted, or smoothing that makes the route smooth rather than a combination of straight lines, It is finally determined (flattening and smoothing are also performed here on a multidimensional coordinate space). A known method can be used for these flattening and smoothing. For this reason, detailed description about these flattening and smoothing is abbreviate | omitted here.

決定された二次元座標空間θｘ−θｙ上の経路は、作業区間上の経路に変換される。最終的な経路は、経路出力手段３によって出力される。なお、スタート位置Ｓ側の経路とゴール位置Ｇ側の経路のどちらかが早く中間点Ｍｉに達し、かつ、さらに中間点Ｍｉ＋１を設定する必要が生じた場合には、中間点Ｍｉは経路点の一つとして次の中間点Ｍｉ＋１が設定される。上述したように、複数のゴール位置Ｇを設定して上述したような経路生成を行うことで、滑らかな経路設定を実現することができる。また、中間点Ｍｉの設定時に予め複数の候補ｍｉを設定してから可操作度に基づいて一つを選択／決定することで、自動機械（ここではロボットアーム１００）に無理な動きや姿勢をさせないで済む。 The determined route on the two-dimensional coordinate space θ x −θ y is converted into a route on the work section. The final route is output by the route output means 3. If one of the path on the start position S side and the path on the goal position G side reaches the intermediate point M i earlier and the intermediate point M i + 1 needs to be set, the intermediate point M i is The next intermediate point M i + 1 is set as one of the route points. As described above, a smooth route setting can be realized by setting a plurality of goal positions G and generating the route as described above. Further, by setting a plurality of candidates m i in advance when setting the intermediate point M i and then selecting / deciding one based on the manipulability degree, it is possible for the automatic machine (here, the robot arm 100) to move You don't have to be in a posture.

なお、本実施形態では、ゴール位置Ｇｉ側の所定長さ分に関して衝突していると判定された場合は、その所定長さ分の始点に戻って破棄している。ただし、衝突していると判定される直前の微小区間までは経路として利用しても良い。また、本実施形態では、ゴール位置Ｇｉ側の所定長さ分に関して衝突していると判定された場合は、それに対応するスタート位置Ｓ側の衝突判定は行わずに次の中間点Ｍｉ＋１を設定することとしているが、対応するスタート側Ｓの衝突判定を行い、衝突していなければそれを経路として採用するようにしても良い。さらに、スタート位置Ｓ側の所定長さ分に関して衝突していると判定された場合は、それに対応する既に終了しているゴール位置Ｇｉ側の経路は利用しても良いし、破棄しても良い。 In the present embodiment, when it is determined that the collision for a given length of the goal position G i side to discard back to the start point of the predetermined length min. However, you may use as a path | route to the micro area immediately before determining with having collided. Further, in the present embodiment, when it is determined that the collision for a given length of the goal position G i side, following the midpoint M i + 1 without the collision determination of the start position S side corresponding thereto Although it is set, the collision determination of the corresponding start side S is performed, and if it does not collide, it may be adopted as the route. Furthermore, if it is determined that the collision for a given length of the start position S side, to previously path goal position G i side has been completed may be used corresponding thereto be discarded good.

本発明の経路計画方法は上述した実施形態に限定されるものではない。例えば、上述した実施形態では、説明を分かりやすくするため、自動機械（ロボットアーム１００）の状態（姿勢）を座標軸上の一点として表す多次元座標軸を二次元としたが、三次元以上の多次元座標軸であっても構わない。また、自動機械は、ロボットアームではなくても良い。さらに、計画する経路は、ロボットアームなどの自動機械の一部ではなく、移動可能な自動機械の移動経路であっても良い。
 ROUTE planning method of the present invention is not limited to the embodiments described above. For example, in the above-described embodiment, the multidimensional coordinate axis representing the state (posture) of the automatic machine (robot arm 100) as one point on the coordinate axis is two-dimensional for easy understanding, but the three-dimensional or more multidimensional It may be a coordinate axis. Further, the automatic machine may not be a robot arm. Furthermore, the planned route is not a part of an automatic machine such as a robot arm, but may be a movement route of a movable automatic machine.

本発明の経路計画方法の一実施形態を実施する装置の構成図である。It is a schematic diagram of an apparatus for implementing an embodiment of the route planning method of the present invention. 本発明の経路計画方法の一実施形態による経路計画のフローチャートである。It is a flowchart of a route planning according to one embodiment of the route planning method of the present invention. 本発明の経路計画方法の一実施形態によって制御される多関節ロボットアームを示す図である。It is a diagram illustrating a multi-joint robot arm to be controlled by an embodiment of the route planning method of the present invention. 経路生成時（１）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (1). 経路生成時（２）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (2). 経路生成時（３）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (3). 経路生成時（４）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (4). 経路生成時（５）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (5). 経路生成時（６）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (6). 経路生成時（７）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (7). 経路生成時（８）の多次元座標空間を示す図である。It is a figure which shows the multidimensional coordinate space at the time of path | route production | generation (8).

符号の説明Explanation of symbols

１…経路生成手段、２…外部環境情報入力手段、３…経路出力手段、４…環境情報記憶手段、５…スタート・ゴール位置決定手段、６…中間点生成手段、１００…多関節ロボットアーム、１０１…リンク、１０２…関節ジョイント、１０３…把持部。   DESCRIPTION OF SYMBOLS 1 ... Path | route production | generation means, 2 ... External environment information input means, 3 ... Path | route output means, 4 ... Environment information storage means, 5 ... Start / goal position determination means, 6 ... Intermediate point generation means, 100 ... Articulated robot arm, 101 ... Link, 102 ... Joint joint, 103 ... Gripping part.

Claims (7)

初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に経路を計画する経路計画方法において、
一つの前記初期位置に対し複数の前記最終位置を予め設定し、前記中間位置を介して前記初期位置と各前記最終位置とを繋ぐ複数の候補となる経路を生成することを特徴とする経路計画方法。 In a route planning method for generating an intermediate point stochastically between an initial position and a final position, and planning a route from the initial position to the final position using the intermediate point,
A route plan characterized in that a plurality of final positions are preset for one initial position, and a plurality of candidate paths connecting the initial position and each final position are generated via the intermediate position. Method. 前記初期位置及び前記最終位置の双方からそれぞれ前記経路を生成するもので、前記最終位置からの前記経路は、（１）少なくとも一つの中間点を生成し、（２）生成された前記中間点に基づいて複数の前記最終位置から一つを選択し、（３）前記中間点と選択された前記最終位置との間に前記経路の少なくとも一部を生成する、ことを少なくとも一回以上行うことで生成されることを特徴とする請求項１に記載の経路計画方法。   The path is generated from both the initial position and the final position, and the path from the final position (1) generates at least one intermediate point and (2) the generated intermediate point. Selecting at least one part of the path between the intermediate point and the selected final position, by selecting one from a plurality of the final positions based on The route planning method according to claim 1, wherein the route planning method is generated. 初期位置と最終位置との間に中間点を確率的に生成させ、該中間点を用いて前記初期位置から前記最終位置までの間に、自動機械の全体又は一部分の経路を計画する経路計画方法において、
前記自動機械が存在する実空間上に作業空間を定義すると共に、該作業空間における前記自動機械の状態が一点として表される多次元座標空間を定義し、
前記作業空間上の前記初期位置と前記最終位置との間の前記経路設定時に、前記多次元座標空間上における前記初期位置に対応する点と前記最終位置に対応する点とを結ぶ経路を、前記作業空間における距離と前記多次元座標空間における距離とを用いて生成することを特徴とする経路計画方法。 A path planning method for probabilistically generating an intermediate point between an initial position and a final position, and planning a path of the whole or a part of the automatic machine between the initial position and the final position using the intermediate point In
Defining a work space on the real space in which the automatic machine exists, and defining a multidimensional coordinate space in which the state of the automatic machine in the work space is represented as one point;
A path connecting a point corresponding to the initial position on the multi-dimensional coordinate space and a point corresponding to the final position at the time of setting the path between the initial position and the final position on the work space, A route planning method characterized by generating using a distance in a work space and a distance in the multidimensional coordinate space. 前記作業空間における距離を用いて前記中間点の設定を行い、該中間点に向けた前記経路の生成を前記多次元座標空間における距離を用いて決定することを特徴とする請求項３に記載の経路計画方法。   The intermediate point is set using a distance in the work space, and the generation of the route toward the intermediate point is determined using the distance in the multidimensional coordinate space. Path planning method. 前記経路は、（１）少なくとも一つの中間点を前記多次元座標空間上に生成し、（２）該多次元座標空間上における前記初期位置又は前記最終位置あるいは既に生成されている前記経路上の点の中から、生成された前記中間点と前記作業空間上で最も近い距離にある点を経路点として選択し、（３）前記多次元座標空間における距離を用いて、前記経路点から前記中間点への間で障害物と生成中の経路との衝突判定を行い、（４）衝突がない場合に次の経路点を設定する、ことを少なくとも一回以上行うことで、生成されることを特徴とする請求項４に記載の経路計画方法。   The path (1) generates at least one intermediate point on the multidimensional coordinate space, and (2) the initial position or the final position on the multidimensional coordinate space or the path that has already been generated. A point closest to the generated intermediate point on the work space is selected as a route point from among the points, and (3) using the distance in the multidimensional coordinate space, the intermediate point is selected from the route point. It is determined that a collision between an obstacle and a path being generated between points is performed, and (4) when there is no collision, the next path point is set at least once. The route planning method according to claim 4, wherein the route planning method is characterized in that: 前記中間点の候補となる中間点候補が同時に複数生成され、複数の該中間点候補の中から所定の選択基準に基づいて一つの中間点を選択することを特徴とする請求項１又は３記載の経路計画方法。 Is pre Symbol plurality of candidates to become intermediate point candidate midpoint simultaneously generated, according to claim 1 or 3, characterized in that selecting one of the intermediate points on the basis of a plurality of the intermediate point candidate to a predetermined selection criterion The described route planning method. 前記選択基準が、運動学的可操作性又は動力学的可操作性に基づくものであることを特徴とする請求項６に記載の経路計画方法。
The route planning method according to claim 6, wherein the selection criterion is based on kinematic maneuverability or dynamic maneuverability.
JP2004228461A 2004-08-04 2004-08-04 Route planning method Expired - Fee Related JP4304495B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2004228461A JP4304495B2 (en) 2004-08-04 2004-08-04 Route planning method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2004228461A JP4304495B2 (en) 2004-08-04 2004-08-04 Route planning method

Publications (2)

Publication Number Publication Date
JP2006048372A JP2006048372A (en) 2006-02-16
JP4304495B2 true JP4304495B2 (en) 2009-07-29

Family

ID=36026848

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2004228461A Expired - Fee Related JP4304495B2 (en) 2004-08-04 2004-08-04 Route planning method

Country Status (1)

Country Link
JP (1) JP4304495B2 (en)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2006121091A1 (en) 2005-05-13 2006-11-16 Toyota Jidosha Kabushiki Kaisha Path planning device
JP5044991B2 (en) * 2006-05-25 2012-10-10 トヨタ自動車株式会社 Route creation apparatus and route creation method
JP4941068B2 (en) * 2007-04-16 2012-05-30 トヨタ自動車株式会社 Route creation method and route creation device
JP2009032189A (en) * 2007-07-30 2009-02-12 Toyota Motor Corp Device for generating robot motion path
JP5448326B2 (en) * 2007-10-29 2014-03-19 キヤノン株式会社 Gripping device and gripping device control method
JP5137537B2 (en) * 2007-11-28 2013-02-06 三菱電機株式会社 Robot work motion optimization device
JP2009172685A (en) * 2008-01-22 2009-08-06 Yaskawa Electric Corp Manipulator system and its control method
JP5002609B2 (en) * 2009-03-17 2012-08-15 株式会社東芝 Trajectory generation system for mobile manipulator
WO2011070869A1 (en) * 2009-12-07 2011-06-16 国立大学法人東京大学 Mobile body system
JP5361004B2 (en) * 2010-09-13 2013-12-04 独立行政法人国立高等専門学校機構 Smooth path generation device and smooth path generation method
JP5659890B2 (en) * 2011-03-14 2015-01-28 トヨタ自動車株式会社 Robot trajectory planning system and trajectory planning method
JP5803179B2 (en) * 2011-03-18 2015-11-04 株式会社デンソーウェーブ Robot control method and robot control apparatus
JP2014240106A (en) * 2013-06-12 2014-12-25 セイコーエプソン株式会社 Robot, robot control device, and driving method of robot
CN114252070A (en) * 2020-09-25 2022-03-29 海鹰航空通用装备有限责任公司 Unmanned aerial vehicle path planning method suitable for given path length
CN113146637B (en) * 2021-04-29 2022-11-25 张耀伦 Robot Cartesian space motion planning method
CN115302357B (en) * 2022-08-05 2023-05-16 中国人民解放军空军工程大学航空机务士官学校 Spiral polishing path planning method based on evaluation function

Also Published As

Publication number Publication date
JP2006048372A (en) 2006-02-16

Similar Documents

Publication Publication Date Title
JP4941068B2 (en) Route creation method and route creation device
JP4304495B2 (en) Route planning method
US8825209B2 (en) Method and apparatus to plan motion path of robot
US20110035087A1 (en) Method and apparatus to plan motion path of robot
US20110106306A1 (en) Path planning apparatus of robot and method and computer-readable medium thereof
US9044862B2 (en) Path planning apparatus and method for robot
KR101732902B1 (en) Path planning apparatus of robot and method thereof
US8219246B2 (en) System and method for controlling a robotic arm
JP5044991B2 (en) Route creation apparatus and route creation method
JP6170285B2 (en) Manipulator and its path generation method
JP5659890B2 (en) Robot trajectory planning system and trajectory planning method
CN114137951A (en) Online robot motion planning framework
JP2009134352A (en) Robot motion path creating device, and robot motion path creating method
JP4667764B2 (en) Route setting method
Rajendran et al. User-guided path planning for redundant manipulators in highly constrained work environments
JP4760732B2 (en) Route creation device
Waltersson et al. Planning and control for cable-routing with dual-arm robot
JP2007000954A (en) Robot teaching device and method
EP2304509A2 (en) Method of controlling a robotic arm
JP2005007486A (en) Gripping control device of robot hand
JP4873254B2 (en) Robot direct teaching device
Weber et al. An improved approach for inverse kinematics and motion planning of an industrial robot manipulator with reinforcement learning
Brooks et al. The Predictive Kinematic Control Tree: Enhancing Teleoperation of Redundant Robots through Probabilistic User Models
JP2019155509A (en) Robot controlling device, control method, and program
Chen et al. Transferring autonomous reaching and targeting behaviors for cable-driven robots in minimally invasive surgery

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20070410

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20080604

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20080701

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20080829

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: 20090401

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20090414

R151 Written notification of patent or utility model registration

Ref document number: 4304495

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20120515

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20120515

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20130515

Year of fee payment: 4

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20130515

Year of fee payment: 4

LAPS Cancellation because of no payment of annual fees