JP2005309990A - Path setting method - Google Patents

Path setting method Download PDF

Info

Publication number
JP2005309990A
JP2005309990A JP2004128866A JP2004128866A JP2005309990A JP 2005309990 A JP2005309990 A JP 2005309990A JP 2004128866 A JP2004128866 A JP 2004128866A JP 2004128866 A JP2004128866 A JP 2004128866A JP 2005309990 A JP2005309990 A JP 2005309990A
Authority
JP
Japan
Prior art keywords
path
time
route
joint
setting
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
Application number
JP2004128866A
Other languages
Japanese (ja)
Other versions
JP4667764B2 (en
Inventor
Shintaro Yoshizawa
真太郎 吉澤
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 JP2004128866A priority Critical patent/JP4667764B2/en
Publication of JP2005309990A publication Critical patent/JP2005309990A/en
Application granted granted Critical
Publication of JP4667764B2 publication Critical patent/JP4667764B2/en
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Images

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

<P>PROBLEM TO BE SOLVED: To provide a path setting method by which an automatic machine is smoothly operated and contribute reduction of an amount of calculation. <P>SOLUTION: The path setting method is characterized by providing an intermediate point setting process for tentatively setting a plurality of intermediate points p<SB>1</SB>, p<SB>2</SB>, p<SB>3</SB>, etc. between the initial and the last positions of a path setting target part by considering the whole or part of the automatic machine as the path setting target part, a time setting process for setting time t<SB>1</SB>to t<SB>2</SB>, t<SB>2</SB>to t<SB>3</SB>, etc. in which the path setting target part moves between the intermediate points p<SB>1</SB>, p<SB>2</SB>, etc., a planarization process for planarizing a path by extracting and omitting omissible intermediate points from among the plurality of intermediate points p<SB>1</SB>, p<SB>2</SB>, etc., a smoothing process for performing smoothing by changing the vicinity of each intermediate point of the path after planarization to the smooth path and a path correction process for deciding whether or not the path after smoothing interferes with an obstacle and correcting the path by locally changing the path when the path interferes with the obstacle. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

本発明は、自動機械の全体又は一部分の移動経路を設定する経路設定方法に関する。   The present invention relates to a route setting method for setting a movement route of all or a part of an automatic machine.

ロボットを移動させる際や、ロボットアームを自動操作する際には、その経路を設定する必要が生じる。下記［特許文献１］には、空間内でのロボットの望ましい経路を決定する方法として、空間的・時間的な経路を関節角の関数として記述する関節空間法や、デカルト座標系での位置と姿勢の経路とを時間の関数として記述するデカルト空間法などが記載されている。関節空間法では、経路のスタート位置（初期位置）とゴール位置（最終位置）との間に一以上の中間点を設定し、これらをスプライン曲線で接続した経路を作成する。この経路によって決まる速度及び加速度を制御するために、多項式スプライン関数を用いる方法も下記［特許文献１］に紹介されている。
ロボティクス−機構・力学・制御−（第７章）、Ｊｏｈｎ Ｊ．Ｃｒａｉｇ 著、三浦宏文・下山勲 訳、共立出版株式会社、１０９６４１号公報 Ｒａｎｄｏｍｉｚｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ： Ｓ．Ｍ．ＬａＶａｌｌｅ ａｎｄ Ｊ．Ｊ．Ｋｕｆｆｎｅｒ，Ｊｒ． ： Ｐｒｏｃ ｏｆ ＩＥＥＥ Ｉｎｔ． Ｃｏｎｆ． ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ， １９９９ When moving the robot or automatically operating the robot arm, it is necessary to set the path. In [Patent Document 1] below, as a method for determining a desired path of a robot in space, a joint space method that describes a spatial / temporal path as a function of a joint angle, a position in a Cartesian coordinate system, A Cartesian space method for describing the path of the posture as a function of time is described. 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. A method using a polynomial spline function to control the speed and acceleration determined by this path is also introduced in [Patent Document 1] below.
Robotics-Mechanism, Mechanics, Control-(Chapter 7), John J. et al. Craig, translated by Hirofumi Miura and Isao Shimoyama, Kyoritsu Publishing Co., Ltd., 109641 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

［特許文献１］には、各点（初期位置・中間点・最終位置）を時間の関数として繋ぐ多項式スプライン補間として、特に３次多項式と５次多項式を用いた場合が紹介されている。しかし、３次多項式スプラインは速度の滑らかさまでは保証できるが、加速度の滑らかさまでは一般に保証できない。このため、３次多項式スプラインを用いて生成された経路に従って多関節ロボットアームを制御した場合に、加速度の滑らかさの欠如に起因する振動が発生し得る。そこで、［特許文献１］には、これを改善するために５次多項式スプライン補間も紹介されているが、中間点の補間後に加速度−時間グラフにおいて余計な凹凸の個数が増加する。   [Patent Document 1] introduces, in particular, a case where a cubic polynomial and a fifth polynomial are used as polynomial spline interpolation that connects points (initial position, intermediate point, and final position) as a function of time. However, cubic polynomial splines can be guaranteed with smooth speed, but generally cannot be guaranteed with smooth acceleration. For this reason, when the articulated robot arm is controlled according to a path generated using a cubic polynomial spline, vibration due to lack of smoothness of acceleration may occur. [Patent Document 1] introduces quintic polynomial spline interpolation to improve this, but the number of extra irregularities in the acceleration-time graph increases after interpolation of the intermediate points.

また、複数の障害物が配置された作業空間においてロボットアームなどの自動機械の移動経路を計画する場合、設定した経路が障害物と干渉するものであった場合は、再計算が必要となる。ここで、経路設定を最初から全てやり直す必要があると計算量が増えてしまう。このため、設定した経路が障害物と干渉する場合には、その部分（位置・時間）近傍を局所的に修正できると計算量の低減に効果がある。しかし、多項式スプライン補間では、多項式スプライン曲線が、ある時刻で障害物と干渉する場合、当該時刻近傍で局所的に修正することは一般に不可能である。従って、本発明の目的は、自動機械を円滑に動作させることができ、かつ、計算量の低減にも寄与する経路設定方法を提供することにある。   Further, when planning the movement path of an automatic machine such as a robot arm in a work space where a plurality of obstacles are arranged, recalculation is required if the set path interferes with the obstacle. Here, if it is necessary to redo all the route settings from the beginning, the amount of calculation increases. For this reason, when the set route interferes with an obstacle, if the vicinity of the portion (position / time) can be locally corrected, the amount of calculation can be reduced. However, in polynomial spline interpolation, when a polynomial spline curve interferes with an obstacle at a certain time, it is generally impossible to correct locally in the vicinity of the time. Accordingly, an object of the present invention is to provide a path setting method that can smoothly operate an automatic machine and contribute to a reduction in calculation amount.

請求項１に記載の経路設定方法は、自動機械の全体又は一部分の移動経路を設定するもので、自動機械の全体又は一部分を経路設定対象部として、該経路設定対象部の初期位置から最終位置の間に複数の中間点を仮設定する中間点設定行程と、経路設定対象部が中間点の間を移動する時間を設定する時間設定行程と、複数の中間点の中から省略可能な中間点を抽出して省略することで仮設定された経路を平坦化する平坦化行程と、中間点を直線的に結んだ状態の平坦化後の経路の各中間点近傍を滑らかな経路に変更することで平滑化する平滑化行程と、平滑化後の経路が障害物と干渉するか否かを判定し、障害物と干渉する場合は経路上の障害物との干渉部分近傍を局所的に経路変更することで経路修正する経路修正行程とを備えていることを特徴としている。   The route setting method according to claim 1 sets a movement route of the whole or a part of the automatic machine, and uses the whole or a part of the automatic machine as a route setting target unit, from an initial position of the route setting target unit to a final position. An intermediate point setting process for temporarily setting a plurality of intermediate points in between, a time setting process for setting a time for the path setting target part to move between the intermediate points, and an intermediate point that can be omitted from a plurality of intermediate points By extracting and omitting, the process of flattening the temporarily set path and changing the vicinity of each intermediate point of the flattened path in a state where the intermediate points are linearly connected to each other to a smooth path Determine whether the smoothing process to smooth and the route after smoothing interfere with the obstacle, and if it interferes with the obstacle, the route is changed locally in the vicinity of the interference part with the obstacle on the route And a route correction process for correcting the route by It is characterized.

請求項２に記載の発明は、請求項１に記載の経路設定方法において、自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する際に、時間設定行程において、各関節ジョイント部分の最大角速度を考慮して、各中間点間の移動時間が設定されることを特徴としている。なお、各関節ジョイント部分の角度・角速度・角加速度とは、当該関節ジョイントに接続される一対のリンクのなす角度と、この角度に関する角速度及び角加速度のことを指す。   According to a second aspect of the present invention, in the path setting method according to the first aspect, the automatic machine is an arm in which a plurality of link members are joined to each other via joint joints, and the movement path of the arm is set. In addition, in the time setting process, the moving time between the intermediate points is set in consideration of the maximum angular velocity of each joint joint portion. Note that the angles, angular velocities, and angular accelerations of the joint joint portions refer to angles formed by a pair of links connected to the joint joints, and angular velocities and angular accelerations related to these angles.

請求項３に記載の発明は、請求項１又は２に記載の経路設定方法において、自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する際に、平滑化行程において、各関節ジョイント部分の角度を時間の関数として捉え、時間の関数である各関節ジョイントの角度を無限回微分可能な関数で近似することで平滑化を行うことを特徴としている。   According to a third aspect of the present invention, in the route setting method according to the first or second aspect, the automatic machine is an arm in which a plurality of link members are joined to each other via a joint joint, and the movement path of the arm is set. In the smoothing process, the angle of each joint joint part is regarded as a function of time, and smoothing is performed by approximating the angle of each joint joint as a function of time with a function that can be differentiated infinitely. It is a feature.

請求項４に記載の発明は、請求項３に記載の経路設定方法において、各関節ジョイント部分の角加速度が所定の最大加速度を超えた場合に、最大加速度を超える時刻近傍において局所的に時間設定変更を行う時間修正行程をさらに備えていることを特徴としている。   According to a fourth aspect of the present invention, in the route setting method according to the third aspect, when the angular acceleration of each joint joint part exceeds a predetermined maximum acceleration, the time is set locally in the vicinity of the time exceeding the maximum acceleration. It is characterized by further comprising a time correction process for making a change.

請求項１に記載の経路設定方法によれば、平坦化の後に平滑化を行うことで、滑らかな移動が可能な経路を設定することができる。また、平滑化後の経路が障害物と干渉する場合には、局所的に修正（位置的修正及び時間的修正）を行うことで、計算量を低減しつつ、経路の修正を容易に行うことができる。   According to the route setting method of the first aspect, it is possible to set a route capable of smooth movement by performing smoothing after flattening. In addition, when the route after smoothing interferes with an obstacle, it is possible to easily correct the route while reducing the amount of calculation by performing local correction (positional correction and temporal correction). Can do.

請求項２に記載の経路設定方法によれば、各関節ジョイント部分の最大角速度を考慮して、ある中間点から他の中間点に移動するまでの時間が決定されるため、各関節ジョイント部分での角速度が最大角速度を超えることを抑制でき、位置的・時間的の双方で滑らかな移動経路を設定することができる。   According to the route setting method according to claim 2, since the time until moving from one intermediate point to another intermediate point is determined in consideration of the maximum angular velocity of each joint joint portion, Can be prevented from exceeding the maximum angular velocity, and a smooth movement path can be set both in terms of position and time.

請求項３に記載の経路設定方法によれば、平滑化を、各関節ジョイント部の角度（時間関数）を無限回微分可能な関数で近似することで、時間関数となる角速度及び角加速度も滑らかな平滑化を行うことができる。   According to the path setting method according to claim 3, smoothing is also performed by approximating the angle (time function) of each joint joint portion with a function that can be differentiated infinitely, so that the angular velocity and the acceleration serving as the time function are also smoothed. Smoothing can be performed.

請求項４に記載の経路設定方法によれば、各関節ジョイント部分での角加速度に関しても最大加速度を考慮し、最大加速度を超えるような場合は時間設定を変更して修正を行うので、初期位置から最終位置までの間で角加速度が過剰となることを抑止できる。   According to the route setting method of the fourth aspect, the maximum acceleration is taken into consideration with respect to the angular acceleration at each joint joint portion, and if the maximum acceleration is exceeded, the time setting is changed and correction is performed. It is possible to prevent the angular acceleration from becoming excessive between the first position and the final position.

本発明の経路設定方法の一実施形態について以下に説明する。まず、本実施形態の経路設定方法を演算する装置（経路生成手段）の構成を図１に示す。経路生成手段１は、具体的にはコンピュータによって構成されている。図１に示されるように、経路生成手段１は外部環境情報を入力する手段である外部環境情報入力手段２と、生成過程にある経路や最終的に生成した（設定した）経路を出力する経路出力手段３とを有している。   An embodiment of the route setting method of the present invention will be described below. First, FIG. 1 shows the configuration of an apparatus (route generation means) that calculates the route setting method of the present embodiment. 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 includes intermediate point generation means 6 (temporary setting), time setting means 7, flattening means 8, smoothing means 9, correction means 10, and the like.

時間設定手段７は、ある中間点から次の中間点までの移動にかかる時間を設定する。ここでは、ある中間点から次の中間点までの単なる位置の変化だけでなく、移動にかかる時間をも考慮する。この結果、追って説明する角速度や角加速度に関する配慮も行えるようになる。平坦化手段８は、中間点生成手段６によって仮設定された中間点の数を減らし、経路が単純になるように平坦化する役割を担っている。平滑化手段９は、平坦化後の中間点間を直線で結んだだけの経路を滑らかな曲線に平滑化する役割を担っている。修正手段１０は、平滑化後の経路に対して、様々な条件と照らして修正が必要であるか否かを判断し、必要であれば修正を行う役割を担っている。修正が必要な条件としては、障害物と干渉する場合や、後述する角速度や角加速度が所定の上限値を超えている場合などがある。これらの中間点生成手段６〜修正手段１０などは、ハードディスク内やＲＯＭ内に格納されたプログラムとこれを実行するＣＰＵなどによって実現されている。   The time setting means 7 sets the time required for movement from one intermediate point to the next intermediate point. Here, not only a simple change in position from one intermediate point to the next intermediate point, but also the time required for movement is considered. As a result, it becomes possible to consider the angular velocity and angular acceleration described later. The flattening means 8 plays a role of reducing the number of intermediate points temporarily set by the intermediate point generating means 6 and flattening so that the path becomes simple. The smoothing means 9 plays a role of smoothing a path that only connects the intermediate points after flattening with a straight line into a smooth curve. The correction means 10 plays a role of determining whether or not the route after the smoothing needs to be corrected in light of various conditions, and correcting the route if necessary. Conditions that need to be corrected include interference with an obstacle, and angular velocity and angular acceleration described later exceeding a predetermined upper limit. These intermediate point generation means 6 to correction means 10 are realized by a program stored in a hard disk or a ROM and a CPU for executing the program.

次に、経路生成の手順について順を追って説明する。ここでは、自動機械がロボットアームである場合を例にして説明する。図２は、冗長な自由度を持つロボットアーム１００を模式的に示した図である。このロボットアーム１００は、図２に示されるように、複数のリンク（線状の部材）１０１と、二つのリンク１０１の間に配置された関節ジョイント１０２とからなっている。また、アーム１００の先端には、把持部１０３が取り付けられている。ここでは、把持部１０３の根本、即ち、最も末端側のリンク１０１の先端部分（手首位置Ｘ）の移動軌跡を生成する。   Next, the route generation procedure will be described in order. Here, a case where the automatic machine is a robot arm will be described as an example. FIG. 2 is a diagram schematically showing a robot arm 100 having redundant degrees of freedom. As shown in FIG. 2, the robot arm 100 includes a plurality of links (linear members) 101 and a joint joint 102 disposed between the two links 101. 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. When the x and y coordinate axes are set for a space in which the robot arm 100 is located, the coordinates (x, y) of the wrist position X are the angles θ 1 , θ 2 , θ at the joint joints in FIG. It can be easily obtained by coordinate conversion of the coordinate position of the vector (θ 1 , θ 2 , θ 3 ) consisting of 3 . Note that FIG. 2 illustrates an example in which the robot arm 100 moves only in the x and y planes for ease of explanation, but it goes without saying that the same idea can be applied to a three-dimensional 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 accumulated 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. 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.

スタート・ゴール位置決定手段５は、環境情報記憶手段４に蓄積された情報と照らし合わせて、入力されたスタート・ゴール位置が障害物と干渉していないか否かを判断し、干渉していない場合はこれらをスタート・ゴール位置として決定する。なお、干渉の有無の検証は、既存の手法を用いることができる。もし、入力されたスタート・ゴール位置が障害物と干渉している場合はオペレータに対して再入力を促し、再入力されたスタート・ゴール位置について同様の判定を行い、スタート・ゴール位置が決定されるまで反復される。次に、公知の手法によって、スタート・ゴール位置の間に中間点が仮設定される（中間点設定行程）。この中間点の仮設定は、中間点生成手段６によって行われる。この中間点の設定方法としては、例えば、［特許文献２］に記載の確率経路探索方法などによって中間点が設定される。   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 the start / goal positions. In addition, the verification of the presence or absence of interference can use the existing method. If the entered start / goal position interferes with an obstacle, the operator is prompted to re-enter and the same determination is made for the re-entered start / goal position to determine the start / goal position. Repeat until Next, an intermediate point is temporarily set between the start and goal positions by a known method (intermediate point setting process). The temporary setting of the intermediate point is performed by the intermediate point generating unit 6. As an intermediate point setting method, for example, the intermediate point is set by a probabilistic route search method described in [Patent Document 2].

なお、ここでは、最終的には手首位置Ｘでの経路を決定するため、上述した中間点は手首位置Ｘについての中間点が重要であるが、これは各関節ジョイント部分に関してスタート位置とゴール位置との中間点が設定されることによって決定される。このとき、各関節の中間点が同時に仮設定された状態では、経路についての位置のみが仮設定されただけである。即ち、ロボットアーム１００の中間姿勢が複数生成されることとなる。これら複数の中間姿勢を時間的な要素を考慮してつなぐため、ある中間点から次の中間点まで移動するまでの時間を設定する（時間設定行程）。この時間設定は、時間設定手段７によって行われる。   Here, in order to finally determine the route at the wrist position X, the intermediate point described above is important for the wrist position X, but this is the start position and the goal position for each joint joint portion. Is determined by setting an intermediate point. At this time, in a state where the intermediate points of the joints are temporarily set at the same time, only the position on the route is temporarily set. That is, a plurality of intermediate postures of the robot arm 100 are generated. In order to connect the plurality of intermediate postures in consideration of time factors, a time until moving from one intermediate point to the next intermediate point is set (time setting process). This time setting is performed by the time setting means 7.

ここでは、ある関節ジョイント部分を例にして説明する。なお、全ての関節ジョイント部分についての経路が決定されれば、手首位置Ｘの経路が決定される。ある関節ジョイントの角度について、スタート位置での角度をθｓ、ゴール位置の角度をθｇとする。そして、上述した中間点生成時に、この関節ジョイント部分に関して仮設定された中間点を関節ジョイントの角度としてθ１，θ２，…，θｎと表すものとする。例えば、ここで、θｓとθ１，θｉとθｉ＋１，θｎとθｇの間の移動にかかる時間を一定時間ｔであると仮定して、関節ジョイントの角度の時間変化をグラフにして示すと図３のようになる。 Here, a certain joint joint portion will be described as an example. In addition, if the path | route about all the joint joint parts is determined, the path | route of the wrist position X will be determined. Regarding the angle of a joint joint, the angle at the start position is θ s , and the angle at the goal position is θ g . At the time of the midpoint generator described above, theta 1 temporarily set the midpoint for this articulation joints as the angle articulation joint, theta 2, ..., and represents a theta n. For example, assuming that the time required to move between θ s and θ 1 , θ i and θ i + 1 , θ n and θ g is a fixed time t, the time change of the joint joint angle is graphed. As shown in FIG.

図３から分かるように、図３の角度変化を示す折れ線の傾きが角速度を表すことになる。ここでは、角速度は時間に対して一定の割合で増減するため、角加速度はゼロである。しかし、ロボットアーム１００を実際に駆動する場合には、関節ジョイント部分のアクチュエータ（モータなど）の機械的性能などから、角速度や角加速度には最大角速度や最大角加速度といった制約が存在する。これらの条件は予め経路生成手段１にインプットされており、これらの制約を考慮して時間設定が行われる。即ち、図３のように、単純に中間点間の移動にかかる時間を決めてしまうと、アクチュエータなどの機械的性能などのために実現し得ない経路設計がなされてしまう場合や、アクチュエータの駆動に不都合がある場合などがある。   As can be seen from FIG. 3, the inclination of the broken line indicating the angle change in FIG. 3 represents the angular velocity. Here, since the angular velocity increases and decreases at a constant rate with respect to time, the angular acceleration is zero. However, when the robot arm 100 is actually driven, there are restrictions on the angular velocity and the angular acceleration such as the maximum angular velocity and the maximum angular acceleration due to the mechanical performance of the actuator (motor and the like) at the joint joint. These conditions are input to the route generation means 1 in advance, and time is set in consideration of these restrictions. That is, as shown in FIG. 3, if the time required for the movement between the intermediate points is simply determined, a path design that cannot be realized due to the mechanical performance of the actuator or the like may be made, or the actuator drive There are cases where there are inconveniences.

そこで、ここでは、以下のように時間設定を行う。各関節ジョイントに関しては上述した制約を考慮して、最大許容角速度ｖｍａｘが予め設定される。そして、θｉからθｉ＋１までの時間間隔（ｔｉ＋１−ｔｉ）として、｜（θｉ＋１−θｉ）／ｖｍａｘ｜で定義される時間を設定すれば、角速度が許容範囲内となる時間設定を行える。二つの関節ジョイント部分における角度・角速度の時間変化の例を図４に示す。なお、図４中のΔｔ１の長さは全ての関節ジョイント部で一致させる必要がある。図４では、関節Ａと関節Ｂとにおける各中間点での制御タイミングがすでに一致されている状態が示されている。 Therefore, here, the time is set as follows. For each joint joint, the maximum allowable angular velocity v max is set in advance in consideration of the above-described restrictions. If the time defined by | (θ i + 1 −θ i ) / v max | is set as the time interval (t i + 1 −t i ) from θ i to θ i + 1 , the time during which the angular velocity is within the allowable range. Can be set. FIG. 4 shows an example of the time change of the angle / angular velocity at the two joint portions. In addition, it is necessary to make the length of Δt 1 in FIG. 4 coincide in all joint joints. FIG. 4 shows a state in which the control timings at the intermediate points of the joint A and the joint B are already matched.

そこで、次に、このΔｔ１の長さの求め方、即ち、複数の関節ジョイント部分での中間点の制御タイミングを合わせる方法について説明する。関節Ａに関しては、Δｔ１の前後で角度がθ１Ａからθ２Ａに変化したとし、関節Ｂに関しては、Δｔ１の前後で角度がθ１Ｂからθ２Ｂに変化したとする。このとき、上述したように、関節Ａに関しては、最大許容速度ｖＡｍａｘが設定されていたとすると、関節Ａでの角速度を許容範囲内とするには、Δｔ１は｜θ２Ａ−θ１Ａ｜／ｖＡｍａｘ以上とする必要がある。同様に、関節Ｂに関しては、最大許容速度ｖＢｍａｘが設定されていたとすると、関節Ｂでの角速度を許容範囲内とするには、Δｔ１は｜θ２Ｂ−θ１Ｂ｜／ｖＢｍａｘ以上とする必要がある。 Therefore, next, a method of obtaining the length of Δt 1 , that is, a method of matching the control timings of the intermediate points at the plurality of joint portions will be described. Assume that for the joint A, the angle changes from θ 1A to θ 2A before and after Δt 1 , and for the joint B, the angle changes from θ 1B to θ 2B before and after Δt 1 . At this time, as described above, assuming that the maximum allowable speed v Amax is set for the joint A, Δt 1 is | θ 2A −θ 1A | / in order to keep the angular velocity at the joint A within the allowable range. It is necessary to set it to v Amax or more. Similarly, regarding the joint B, if the maximum allowable speed v Bmax is set, Δt 1 is set to be equal to or larger than | θ 2B −θ 1B | / v Bmax in order to set the angular velocity at the joint B within the allowable range. There is a need.

そこで、Δｔ１＝ｍａｘ｛｜θ２Ａ−θ１Ａ｜／ｖＡｍａｘ，｜θ２Ｂ−θ１Ｂ｜／ｖＢｍａｘ｝とし、各関節で必要な時間の中で最長の時間を選択してΔｔ１として決定する。ここでは、二つの関節を例にして説明しているが、関節が複数でも同様に行えば良い。また、Δｔ１以降の各時間間隔についても同様にタイミングを一致させる。これで、時間設定が終了する（後で修正される場合もある）。なお、上述した例では、説明を分かりやすくするため、ロボットアーム１００が図２に示されるｘ−ｙ平面内で動く場合を例にして説明しているが、三次元的に動く場合も同様な考え方で時間設定を行えばよい。 Therefore, Δt 1 = max {| θ 2A −θ 1A | / v Amax , | θ 2B −θ 1B | / v Bmax } and select the longest time required for each joint as Δt 1. decide. Here, two joints are described as an example, but the same may be performed even when there are a plurality of joints. In addition, the timing is similarly matched for each time interval after Δt 1 . This completes the time setting (may be corrected later). In the example described above, the case where the robot arm 100 moves in the xy plane shown in FIG. 2 is described as an example for easy understanding. However, the same applies to a case where the robot arm 100 moves three-dimensionally. Time setting should be done based on the idea.

次に、平坦化について説明する。平坦化とは、上述したように設定された中間点の中から省略可能な中間点を抽出し、抽出された中間点を削除することである。平坦化することで、経路が単純化されることになり、ロボットアーム１００の動きの無駄が低減（あるいは解消）される（平坦化行程）。平坦化を説明するための図を図５に示す。図５は、ある関節に関して上述した時間設定が行われた後の経路を示す角度−時間のチャートである。以下、この関節を例にして説明するが、最終的には全ての関節（手首位置Ｘを含む）について同様のことが同時に行われる。即ち、各巻ｓねつの角度は、同時刻において作成されるように設定するので、全ての関節（手首位置Ｘを含む）に関して省略可能な場合にのみ、その部分の平坦化を行う。   Next, planarization will be described. Flattening is extracting an intermediate point that can be omitted from the intermediate points set as described above, and deleting the extracted intermediate point. By flattening, the path is simplified, and waste of movement of the robot arm 100 is reduced (or eliminated) (flattening process). FIG. 5 shows a diagram for explaining the planarization. FIG. 5 is an angle-time chart showing a path after the above-described time setting is performed for a certain joint. In the following, this joint will be described as an example, but finally the same thing is simultaneously performed for all the joints (including the wrist position X). That is, since the angle of each winding s is set so as to be created at the same time, the portion is flattened only when it can be omitted for all joints (including the wrist position X).

図５を参照して説明するが、まず、スタート点ｐ１から何点先（２点以上先から端点までの間）までの平坦化を検討するか指定する。この指定は、オペレータによって入力されても良いし、予めプリセットされたデフォルト値が用いられるようにしても良い。ここでは、３点先（ｐ４）までと指定された場合を例にして説明する。このとき、ｐ１とｐ４とを線分で結び、この線分上のある一定間隔毎の点が障害物と干渉するか否かを検証する。検証の結果、障害物との干渉がない場合は、ｐ１とｐ４とを結んだ経路を新経路とする。 As will be described with reference to FIG. 5, first, the number of points ahead (between two or more points and the end points) from the start point p 1 is specified. This designation may be input by an operator, or a preset default value may be used. Here, a case where up to three points ahead (p 4 ) is designated will be described as an example. At this time, p 1 and p 4 are connected by a line segment, and it is verified whether or not points at certain intervals on the line segment interfere with an obstacle. If there is no interference with the obstacle as a result of the verification, a route connecting p 1 and p 4 is set as a new route.

一方、検証の結果、障害物と干渉する場合は、ｐ１とｐ３とを結んだ経路について同様の検証を行い、障害物との干渉がない場合はｐ１とｐ３とを結んだ経路を新経路とする。ｐ１とｐ３とを結んだ経路が障害物との干渉する場合は、この区間での平坦かは行われない。そして、平坦化後の終点から再度次の平坦化が行われる。例えば、ｐ１とｐ４とを結んだ経路を新経路とした場合は、ｐ４をスタート点として次の平坦化処理が実行される。このようにして順次平坦化が行われる。この平坦化は、平坦化手段８によって行われる。 On the other hand, the result of the verification, if it interferes with the obstacle, verifies same for connecting it routes the p 1 and p 3, if there is no interference with the obstacle connecting the p 1 and p 3 pathway Is a new route. When the path connecting p 1 and p 3 interferes with an obstacle, the flatness in this section is not performed. Then, the next flattening is performed again from the end point after the flattening. For example, when a path connecting p 1 and p 4 is a new path, the next flattening process is executed using p 4 as a start point. In this way, planarization is performed sequentially. This flattening is performed by the flattening means 8.

なお、何点先の点を指定した後、「スタート点から２点先の点から指定点までの間にある各点（指定点が２点先だと一致する）」と「スタート点」とを重複なくランダムに順次線分で結び、各線分に対して障害物との干渉を検証し、障害物と干渉しなかった場合にその線分で平坦化を行うようにしても良い。なお、障害物と干渉しない経路が発見できなかった場合は、その時間区間では平坦化処理は行われない。このようにして、仮設定された経路のスタート位置からゴール位置までの全てに関して平坦化を行う。   In addition, after specifying the number of points ahead, “each point between the point two points after the start point and the specified point (matches if the specified point is two points ahead)” and “start point” May be sequentially connected with a line segment randomly without overlapping, and interference with an obstacle may be verified with respect to each line segment, and when no interference with the obstacle is made, the line segment may be flattened. If a route that does not interfere with the obstacle cannot be found, the flattening process is not performed in that time interval. In this way, flattening is performed for everything from the start position of the temporarily set route to the goal position.

次に、平滑化について説明する。平滑化とは、上述したように設定された経路を滑らかな曲線にすることを指す（平滑化行程）。平坦化が終了した時点での経路は、各中間点において急激に方向が変わるものとなっている。これを、これを滑らかな曲線とすることで、ロボットアーム１００の動きが滑らかとなるようになる。また、ここでは、平滑化後の経路が無限回微分可能な関数によって近似されるため、経路自体の滑らかさに加えて、その経路上の移動速度及び加速度の滑らかさも保証される。さらに、平滑化に際して、滑らかにした経路が障害物と干渉するか検証し、干渉する場合は衝突が回避されるまで平滑化が再履行される。このとき、回避できる経路の存在は平滑化の原理（追って詳しく説明する）によって保証されている。   Next, smoothing will be described. Smoothing refers to making the path set as described above into a smooth curve (smoothing process). The path at the time when the flattening is completed changes direction suddenly at each intermediate point. By making this a smooth curve, the movement of the robot arm 100 becomes smooth. Here, since the route after smoothing is approximated by a function that can be differentiated infinitely, in addition to the smoothness of the route itself, the smoothness of the moving speed and acceleration on the route is also guaranteed. Furthermore, in smoothing, it is verified whether the smoothed path interferes with an obstacle, and in the case of interference, smoothing is performed again until a collision is avoided. At this time, the existence of a path that can be avoided is guaranteed by the principle of smoothing (to be described in detail later).

平滑化に際しては、まず、次の条件（ｉ）〜（ｉｉｉ）を満足するｘの関数ｇ（ｘ）を一つ選んで固定する。
（ｉ） 関数ｇは実ベクトル空間上で定義された無限回微分可能な関数であり、台（ｓｕｐｐｏｒｔ）がコンパクトな非負関数である。
（ｉｉ） 関数ｇの台（ｓｕｐｐｏｒｔ）は実ベクトル空間の原点中心の単位超球の内部に含まれている。
（ｉｉｉ）関数ｇは実ベクトル空間上で密度関数である。
この関数ｇを用いて、正数のパラメータｄに対して、関数ｇｄを下記式（１）によって定義する。なお、下記式（１）中のＮは実ベクトル空間の次元数である。

Figure 2005309990
In smoothing, first, a function g (x) of x that satisfies the following conditions (i) to (iii) is selected and fixed.
(I) The function g is an infinitely differentiable function defined on the real vector space, and the support is a compact non-negative function.
(Ii) The support of the function g is included inside the unit hypersphere at the origin center of the real vector space.
(Iii) The function g is a density function in the real vector space.
Using this function g, the function g d is defined by the following equation (1) for a positive parameter d. In the following equation (1), N is the number of dimensions in the real vector space.
Figure 2005309990

実ベクトル空間Ｖ上で局所可積分な関数をｆとすると、関数ｇｄと関数ｆとの合成積（関数ｆｄ）は下記式（２）のように定義される。

Figure 2005309990
関数ｆに関数ｆｄを対応させる作用素はフリードリスクの軟化子と呼ばれている。このとき、「連続で台がコンパクトな連続関数ｆに対して、ｄが０に近づくとき、関数ｆｄは関数ｆに収束する。」という事実が知られている。この事実を根拠として、区分的に線形な経路（上述した中間点で折れ曲がる経路）の平滑化を行う。 Assuming that f is a local integrable function on the real vector space V, a composite product (function f d ) of the function g d and the function f is defined as the following equation (2).
Figure 2005309990
 An operator that associates the function f with the function f d is called a freed-risk softener. At this time, the fact that “the function f d converges to the function f when d approaches 0 with respect to the continuous function f which is continuous and compact in size” is known. Based on this fact, smoothing of a piecewise linear route (a route that bends at the above-described intermediate point) is performed.

実数上で定義された関数ｇとして、例えば下記（３）式で示される関数ｇ（ｘ）を用いる。この下記式（３）の関数ｇ（ｘ）は、台（ｓｕｐｐｏｒｔ）が−１から１の間に含まれており、実数上で滑らか（無限回微分可能）な関数である。

Figure 2005309990
As the function g defined on the real number, for example, a function g (x) represented by the following equation (3) is used. The function g (x) of the following formula (3) is a function that is smooth (infinitely differentiable) on a real number, with the support being included between −1 and 1.
Figure 2005309990

平滑化を説明するための図を図６に示す。図６では、平坦化後・平滑化前の時間−角度の関係が点線で示されており、平滑化後の時間−角度の関係が実線で示されている。また、ｐ１〜ｐ４が中間点位置での時間と角度の位置を示す点である。まず、平滑化経路の近似誤差を制御する誤差制御変数｛ｄ１，ｄ２，ｄ３，ｄ４…｝の設定方法について説明する。ここでは、誤差制御変数ｄ２について説明するが、他の誤差制御変数も同様の手法によって設定される。 A diagram for explaining the smoothing is shown in FIG. In FIG. 6, the time-angle relationship after smoothing and before smoothing is indicated by a dotted line, and the time-angle relationship after smoothing is indicated by a solid line. Further, p 1 ~p 4 is the point showing the position of the time and angle at the midpoint position. First, a method for setting error control variables {d 1 , d 2 , d 3 , d 4 ...} For controlling the approximation error of the smoothing path will be described. Here is a description of the error control variable d 2, also other errors control variables are set by the same technique.

点ｐｉの座標を（ｔｉ，θｉ）とし、この点を以下ノードと呼ぶこととする。ｐ１−ｐ２の間の時間（ｔ２−ｔ１）とｐ２−ｐ３間の時間（ｔ３−ｔ２）とを比べると、ここでは、（ｔ２−ｔ３）間の時間の方が短い。そこで、誤差制御変数ｄ２の「最大値」を（ｔ２−ｔ１）／２とする。ノードｐ２近傍の平滑化を行う場合には、制御誤差変数としてｄ２を用いて平滑化を行う（式（１）のｄとして誤差制御変数としてｄ２を用いる）。即ち、ノードｐｉ近傍の平滑化を行う際には、誤差制御変数としてｄｉを用いる。なお、平滑化を行うことで障害物と干渉するようになってしまう場合が考えられる。そこで、障害物との干渉を検証しながら平滑化が進められる（経路修正行程）。本実施形態では、平滑化行程と経路修正行程とが一体的に実施される。 The coordinates of the point p i are (t i , θ i ), and this point is hereinafter referred to as a node. When the time between p 1 -p 2 (t 2 -t 1 ) and the time between p 2 -p 3 (t 3 -t 2 ) are compared, here, the time between (t 2 -t 3 ) Is shorter. Therefore, the “maximum value” of the error control variable d 2 is set to (t 2 −t 1 ) / 2. When performing node p 2 near the smoothing (using d 2 as an error control variable as d in Equation (1)) for performing smoothing using d 2 as the control error variable. That is, d i is used as the error control variable when smoothing the vicinity of the node p i . In addition, the case where it will come to interfere with an obstruction by performing smoothing can be considered. Therefore, smoothing is performed while verifying interference with an obstacle (path correction process). In the present embodiment, the smoothing process and the path correction process are integrally performed.

ここで、ｄ２の「最大値」は（ｔ２−ｔ１）／２とされたが、誤差制御変数ｄ２の値が大きいほど、ノードｐ２前後のより広い範囲で平滑化が行われることになり、経路はより滑らかになる。そこで、平滑化処理時には、誤差制御変数ｄ２として小さい値から順次平滑化を行い、障害物と干渉しないもっと大きい値のｄ２で確定する。また、ｄ２の値を大きい値から始めて平滑化を行うようにしても良い。誤差制御変数ｄ２を順次大きくしつつ、できる限り大きな誤差制御変数ｄ２を採用することで、より滑らかな平滑化を行える。なお、図６には、誤差制御変数ｐ２，ｐ３の双方とも、最大値が採用できた場合が示されている。この平滑化は平滑化手段９によって行われる。 Here, the “maximum value” of d 2 is set to (t 2 −t 1 ) / 2, but the larger the value of the error control variable d 2 , the smoothing is performed in a wider range around the node p 2. As a result, the path becomes smoother. Therefore, during the smoothing process, the error control variable d 2 is sequentially smoothed from a small value, and determined with a larger value d 2 that does not interfere with the obstacle. It is also possible to perform a smoothing beginning large value the value of d 2. While sequentially increasing the error control variable d 2, by employing a large error control variable d 2 as possible, enabling a smoother smoothing. FIG. 6 shows a case where the maximum values can be adopted for both error control variables p 2 and p 3 . This smoothing is performed by the smoothing means 9.

上述した手法によれば、時間設定の手法から分かるように角速度が許容し得る最大角速度を超えるようなことはない。しかし、角加速度に関しては、平滑化後に、許容し得る最大角加速度を超えないことは保証されていない。そこで、角速度の最大許容角速度ｖｍａｘと同様に、角加速度についても最大許容角加速度ａｍａｘが予め設定される。そして、角加速度がこの最大許容角加速度ａｍａｘを超えないように、平滑化後の経路が修正される（時間修正行程）。この経路修正は、経路修正手段１０によって行われる。この経路修正行程では、時間スケールが局所的に修正され、これによって角加速度が許容範囲内に納められる。 According to the above-described method, as can be seen from the time setting method, the angular velocity does not exceed the allowable maximum angular velocity. However, with respect to angular acceleration, it is not guaranteed that the maximum allowable angular acceleration will not be exceeded after smoothing. Therefore, similarly to the maximum allowable angular velocity v max of the angular velocity, the maximum allowable angular acceleration a max is set in advance for the angular acceleration. Then, the smoothed path is corrected so that the angular acceleration does not exceed the maximum allowable angular acceleration a max (time correction process). This route correction is performed by the route correction means 10. In this path correction process, the time scale is corrected locally, thereby keeping the angular acceleration within an acceptable range.

なお、以下、ある関節（手首位置Ｘを含む）を例にして説明するが、最終的には全ての関節（手首位置Ｘを含む）について同様のことが行われる。時間−角度、時間−角速度、時間−角加速度の関係を示した例を図７に示す。図７中、点線が平坦化後・平滑化前の経路についての角度・角速度・角加速度を示しており、実線が平滑化後の経路についての角度・角速度・角加速度を示している（角加速度に関しては、平坦化後・平滑化前の経路ではつねに０であるので図示を省略する）。図７からも分かるように、上述したノードを提示する時刻で角加速度は極大値・極小値をとる。ノード近傍の経路は、上述した合成積の関数ｆｄによって近似されているため、ｆｄの時間による２回導関数（∂２／∂ｔ２）ｆｄ（ｔ）を求め、この２回導関数に各ノードを定義する時刻｛ｔ１，ｔ２，ｔ３，…，ｔｎ｝を代入することで、各時刻での角加速度｛（∂２／∂ｔ２）ｆｄ（ｔ１），（∂２／∂ｔ２）ｆｄ（ｔ２），（∂２／∂ｔ２）ｆｄ（ｔ３），…，（∂２／∂ｔ２）ｆｄ（ｔｎ）｝を求めることができる。 In the following description, a certain joint (including the wrist position X) will be described as an example. However, the same applies to all the joints (including the wrist position X) in the end. An example showing the relationship of time-angle, time-angular velocity, and time-angular acceleration is shown in FIG. In FIG. 7, the dotted line indicates the angle, angular velocity, and angular acceleration for the path after flattening and before smoothing, and the solid line indicates the angle, angular velocity, and angular acceleration for the path after smoothing (angular acceleration). Is always 0 in the path after flattening and before smoothing, and the illustration is omitted). As can be seen from FIG. 7, the angular acceleration takes the maximum value and the minimum value at the time when the node is presented. Route node vicinity, because it is approximated by a function f d of convolution as described above, 2 Kaishirube function by time f d (∂ 2 / ∂t 2) determine the f d (t), the 2 Kaishirube By substituting the time {t 1 , t 2 , t 3 ,..., T n } for defining each node into the function, the angular acceleration {(∂ 2 / ∂t 2 ) f d (t 1 ) at each time , (∂ 2 / ∂t 2 ) f d (t 2 ), (∂ 2 / ∂t 2 ) f d (t 3 ), ..., (∂ 2 / ∂t 2 ) f d (t n )} be able to.

そして、求められた各時刻での角加速度｛（∂２／∂ｔ２）ｆｄ（ｔ１），（∂２／∂ｔ２）ｆｄ（ｔ２），（∂２／∂ｔ２）ｆｄ（ｔ３），…，（∂２／∂ｔ２）ｆｄ（ｔｎ）｝のなかから、上述した最大許容角加速度ａｍａｘを超えるものを抽出する。なお、ここで、合成積の積分記号下の微分定理を利用すれば、導関数を容易に求められる。例えば、時刻ｔ２での角加速度ａ２＝（∂２／∂ｔ２）ｆｄ（ｔ２）＞ａｍａｘであった場合は、時刻ｔ２−ｄ２からｔ２＋ｄ２までのΔｔ＝２ｄ２の区間の時間を（ａ２／ａｍａｘ）倍に引き延ばす。なお、ｄ２は、ｔ２近傍で用いられた誤差制御変数である。このように、時間軸を局所的に引き延ばすことで、角加速度は許容範囲内に修正される。 Then, the angular accelerations {(∂ 2 / ∂t 2 ) f d (t 1 ), (∂ 2 / ∂t 2 ) f d (t 2 ), (∂ 2 / ∂t 2 ) at each obtained time From among f d (t 3 ),..., (∂ 2 / ∂ t 2 ) f d (t n )}, those exceeding the above-described maximum allowable angular acceleration a max are extracted. Here, the derivative can be easily obtained by using the differential theorem under the integral symbol of the composite product. For example, the angular acceleration a 2 = (∂ 2 / ∂t 2) at time t 2 f d (t 2) > if it was a max is from time t 2 -d 2 until t 2 + d 2 Δt = The time of the section of 2d 2 is extended by (a 2 / a max ) times. D 2 is an error control variable used in the vicinity of t 2 . Thus, the angular acceleration is corrected within an allowable range by locally extending the time axis.

なお、ある関節について、角加速度条件から時間軸の引き延ばしが行われた場合は、同時刻における他の関節における角加速度が許容範囲内であっても、全ての関節についての時間軸も引き延ばされ、全体としての制御タイミングが一致するようになされる。この後、各関節ジョイント部１０２（手首位置Ｘを含む）に関して角度、角速度、角加速度の時間に関するグラフを作成すれば、全ての関節について角度、角速度、角加速度の滑らかな設計が完了している。   When the time axis is extended from an angular acceleration condition for a certain joint, the time axes for all the joints are extended even if the angular acceleration at other joints is within the allowable range at the same time. Thus, the control timing as a whole is made to coincide. Thereafter, if a graph relating to the time of the angle, angular velocity, and angular acceleration is created for each joint joint portion 102 (including the wrist position X), the smooth design of the angle, angular velocity, and angular acceleration is completed for all the joints. .

なお、本発明は上述した実施形態に限定されるものではない。例えば、上述した本実施形態では、関数ｇ（ｘ）として式（３）で示されたものを用いたが、条件（ｉ）〜（ｉｉｉ）が満たされれば、他の関数を用いても良い。また、上述した実施形態では、多関節ロボットアーム１００の経路を設定する場合を説明した。しかし、本発明は、ロボットアームの移動経路以外にも適用することができ、自動機械の全体の移動経路や、自動機械の一部（上述したアームなど）の移動経路を設定する場合などにも適用することができる。   In addition, this invention is not limited to embodiment mentioned above. For example, in the present embodiment described above, the function g (x) represented by the expression (3) is used, but other functions may be used as long as the conditions (i) to (iii) are satisfied. . In the above-described embodiment, the case where the path of the articulated robot arm 100 is set has been described. However, the present invention can be applied to other than the movement path of the robot arm, and may be used when setting the movement path of the entire automatic machine or the movement path of a part of the automatic machine (such as the arm described above). Can be applied.

本発明の経路設定方法の一実施形態を実施する装置の構成図である。It is a block diagram of the apparatus which implements one Embodiment of the route setting method of this invention. 本発明の経路設定方法の一実施形態によって制御される他関節ロボットアームを示す図である。It is a figure which shows the other joint robot arm controlled by one Embodiment of the path | route setting method of this invention. 各中間点間の時間を一定とした場合の時間−関節角度のグラフである。It is a graph of time-joint angle when time between each intermediate point is made constant. 時間設定後の時間−関節角度のグラフであり、（ａ）は関節Ａについての、（ｂ）は他の関節Ｂについてのグラフである。It is a graph of time-joint angle after time setting, (a) is a graph about joint A, (b) is a graph about other joint B. 平坦化を説明する時間−関節角度のグラフである。It is a graph of time-joint angle explaining flattening. 平滑化を説明する時間−関節角度のグラフである。It is a graph of time-joint angle explaining smoothing. （ａ）時間−角度、（ｂ）時間−角速度、（ｃ）時間−角加速度の関係を示すグラフである。It is a graph which shows the relationship of (a) time-angle, (b) time-angular velocity, and (c) time-angular acceleration.

符号の説明Explanation of symbols

１…経路生成手段、２…外部環境情報入力手段、３…経路出力手段、４…環境情報記憶手段、５…スタート・ゴール位置決定手段、６…中間点生成手段、７…時間設定手段、８…平坦化手段、９…平滑化手段、１０…経路修正手段、１００…多関節ロボットアーム、１０１…リンク、１０２…関節ジョイント、１０３…把持部。   DESCRIPTION OF SYMBOLS 1 ... Path | route production | generation means, 2 ... External environment information input means, 3 ... Path | route output means, 4 ... Environmental information storage means, 5 ... Start / goal position determination means, 6 ... Intermediate point generation means, 7 ... Time setting means, 8 ... flattening means, 9 ... smoothing means, 10 ... path correcting means, 100 ... articulated robot arm, 101 ... link, 102 ... joint joint, 103 ... gripping part.

Claims (4)

自動機械の全体又は一部分の移動経路を設定する経路設定方法において、
前記自動機械の全体又は一部分を経路設定対象部として、該経路設定対象部の初期位置から最終位置の間に複数の中間点を仮設定する中間点設定行程と、
前記経路設定対象部が前記中間点の間を移動する時間を設定する時間設定行程と、
複数の前記中間点の中から省略可能な中間点を抽出して省略することで仮設定された前記経路を平坦化する平坦化行程と、
中間点を直線的に結んだ状態の平坦化後の前記経路の各中間点近傍を滑らかな経路に変更することで平滑化する平滑化行程と、
平滑化後の前記経路が障害物と干渉するか否かを判定し、障害物と干渉する場合は前記経路上の障害物との干渉部分近傍を局所的に変更することで経路修正する経路修正行程とを備えていることを特徴とする経路設定方法。 In a route setting method for setting a movement route of all or part of an automatic machine,
An intermediate point setting step for temporarily setting a plurality of intermediate points between an initial position and a final position of the route setting target unit, using the whole or a part of the automatic machine as a route setting target unit,
A time setting step for setting a time for the route setting target unit to move between the intermediate points;
A flattening step of flattening the temporarily set path by extracting and omitting an omissible intermediate point from the plurality of intermediate points;
A smoothing step of smoothing by changing the vicinity of each intermediate point of the path after flattening in a state where the intermediate points are linearly connected to each other to a smooth path;
It is determined whether or not the route after smoothing interferes with an obstacle, and when it interferes with an obstacle, the route is corrected by locally changing the vicinity of the interference part with the obstacle on the route. And a route setting method. 前記自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する経路設定方法において、
前記時間設定行程において、各関節ジョイント部分の最大角速度を考慮して、各中間点間の移動時間が設定されることを特徴とする請求項１に記載の経路設定方法。 In the path setting method in which the automatic machine is an arm in which a plurality of link members are joined to each other via a joint joint, and the movement path of the arm is set.
2. The route setting method according to claim 1, wherein in the time setting step, a moving time between each intermediate point is set in consideration of a maximum angular velocity of each joint joint portion. 前記自動機械が、複数のリンク部材を関節ジョイントを介して互いに接合したアームであり、該アームの移動経路を設定する経路設定方法において、
前記平滑化行程において、各関節ジョイント部分の角度を時間の関数として捉え、時間の関数である各関節ジョイントの角度を無限回微分可能な関数で近似することで平滑化を行うことを特徴とする請求項１又は２に記載の経路設定方法。 In the path setting method in which the automatic machine is an arm in which a plurality of link members are joined to each other via a joint joint, and the movement path of the arm is set.
In the smoothing step, the angle of each joint joint part is regarded as a function of time, and smoothing is performed by approximating the angle of each joint joint, which is a function of time, with a function that can be differentiated infinitely. The route setting method according to claim 1 or 2. 各関節ジョイント部分の角加速度が所定の最大角加速度を超えた場合に、最大角加速度を超える時刻近傍において局所的に時間設定変更を行う時間修正行程をさらに備えていることを特徴とする請求項３に記載の経路設定方法。   The method further comprises a time correction step of locally changing the time setting in the vicinity of the time exceeding the maximum angular acceleration when the angular acceleration of each joint joint portion exceeds a predetermined maximum angular acceleration. 3. The route setting method according to 3.
JP2004128866A 2004-04-23 2004-04-23 Route setting method Expired - Fee Related JP4667764B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2004128866A JP4667764B2 (en) 2004-04-23 2004-04-23 Route setting method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2004128866A JP4667764B2 (en) 2004-04-23 2004-04-23 Route setting method

Publications (2)

Publication Number Publication Date
JP2005309990A true JP2005309990A (en) 2005-11-04
JP4667764B2 JP4667764B2 (en) 2011-04-13

Family

ID=35438663

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2004128866A Expired - Fee Related JP4667764B2 (en) 2004-04-23 2004-04-23 Route setting method

Country Status (1)

Country Link
JP (1) JP4667764B2 (en)

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006026828A (en) * 2004-07-16 2006-02-02 Toyota Motor Corp Programming method for robot movement
WO2006121091A1 (en) * 2005-05-13 2006-11-16 Toyota Jidosha Kabushiki Kaisha Path planning device
JP2008269021A (en) * 2007-04-16 2008-11-06 Toyota Motor Corp Method and apparatus for creating path
JP2010046752A (en) * 2008-08-21 2010-03-04 Kobe Steel Ltd Method of determining route of robot travel cart, and robot system capable of performing the method
WO2010026710A1 (en) * 2008-09-03 2010-03-11 村田機械株式会社 Route planning method, route planning unit, and autonomous mobile device
WO2014156784A1 (en) * 2013-03-26 2014-10-02 Ntn株式会社 Linking apparatus control device
JP2014188602A (en) * 2013-03-26 2014-10-06 Ntn Corp Control device of link operation device
JP2018069428A (en) * 2016-11-04 2018-05-10 トヨタ自動車株式会社 Operation path planning method
JP2019135076A (en) * 2018-02-05 2019-08-15 キヤノン株式会社 Locus generation method and device
JP2019209457A (en) * 2018-06-08 2019-12-12 トヨタ自動車株式会社 Motion plan apparatus and motion plan method
CN111256722A (en) * 2020-02-28 2020-06-09 广东博智林机器人有限公司 Path planning method, device, equipment and storage medium
JP2021026602A (en) * 2019-08-07 2021-02-22 ファナック株式会社 Movement path generation device
JP2021079482A (en) * 2019-11-19 2021-05-27 株式会社日立製作所 Trajectory plan generation device, trajectory plan generation method, and trajectory plan generation program
WO2022190535A1 (en) * 2021-03-12 2022-09-15 オムロン株式会社 Control device, rack, control system, control method, and program
WO2022196005A1 (en) * 2021-03-15 2022-09-22 オムロン株式会社 Control device, control system, control method, and program
US11458626B2 (en) 2018-02-05 2022-10-04 Canon Kabushiki Kaisha Trajectory generating method, and trajectory generating apparatus
WO2023058243A1 (en) * 2021-10-08 2023-04-13 ファナック株式会社 Control device and computer-readable recording medium storing program

Cited By (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2006026828A (en) * 2004-07-16 2006-02-02 Toyota Motor Corp Programming method for robot movement
JP4613539B2 (en) * 2004-07-16 2011-01-19 トヨタ自動車株式会社 Robot motion planning method
WO2006121091A1 (en) * 2005-05-13 2006-11-16 Toyota Jidosha Kabushiki Kaisha Path planning device
US8055383B2 (en) 2005-05-13 2011-11-08 Toyota Jidosha Kabushiki Kaisha Path planning device
US8290620B2 (en) 2007-04-16 2012-10-16 Toyota Jidosha Kabushiki Kaisha Route creation method and route creation device
JP2008269021A (en) * 2007-04-16 2008-11-06 Toyota Motor Corp Method and apparatus for creating path
JP2010046752A (en) * 2008-08-21 2010-03-04 Kobe Steel Ltd Method of determining route of robot travel cart, and robot system capable of performing the method
US8515612B2 (en) 2008-09-03 2013-08-20 Murata Machinery, Ltd. Route planning method, route planning device and autonomous mobile device
EP2821875A3 (en) * 2008-09-03 2015-05-20 Murata Machinery, Ltd. Route planning method, route planning unit, and autonomous mobile device
WO2010026710A1 (en) * 2008-09-03 2010-03-11 村田機械株式会社 Route planning method, route planning unit, and autonomous mobile device
WO2014156784A1 (en) * 2013-03-26 2014-10-02 Ntn株式会社 Linking apparatus control device
JP2014188602A (en) * 2013-03-26 2014-10-06 Ntn Corp Control device of link operation device
US9808932B2 (en) 2013-03-26 2017-11-07 Ntn Corporation Linking apparatus control device
JP2018069428A (en) * 2016-11-04 2018-05-10 トヨタ自動車株式会社 Operation path planning method
US11458626B2 (en) 2018-02-05 2022-10-04 Canon Kabushiki Kaisha Trajectory generating method, and trajectory generating apparatus
JP2019135076A (en) * 2018-02-05 2019-08-15 キヤノン株式会社 Locus generation method and device
JP7158862B2 (en) 2018-02-05 2022-10-24 キヤノン株式会社 Information processing method and information processing device
CN110576437A (en) * 2018-06-08 2019-12-17 丰田自动车株式会社 Action planning device and action planning method
JP2019209457A (en) * 2018-06-08 2019-12-12 トヨタ自動車株式会社 Motion plan apparatus and motion plan method
JP7067289B2 (en) 2018-06-08 2022-05-16 トヨタ自動車株式会社 Motion planning device and motion planning method
US11167418B2 (en) * 2018-06-08 2021-11-09 Toyota Jidosha Kabushiki Kaisha Operation planning apparatus and operation planning method
JP2021026602A (en) * 2019-08-07 2021-02-22 ファナック株式会社 Movement path generation device
JP7351672B2 (en) 2019-08-07 2023-09-27 ファナック株式会社 Travel route generation device
WO2021100268A1 (en) * 2019-11-19 2021-05-27 株式会社日立製作所 Trajectory plan generation device, trajectory plan generation method, and trajectory plan generation program
JP2021079482A (en) * 2019-11-19 2021-05-27 株式会社日立製作所 Trajectory plan generation device, trajectory plan generation method, and trajectory plan generation program
CN114728418A (en) * 2019-11-19 2022-07-08 株式会社日立制作所 Orbit plan generating device, orbit plan generating method, and orbit plan generating program
JP7262373B2 (en) 2019-11-19 2023-04-21 株式会社日立製作所 Trajectory plan generation device, trajectory plan generation method, and trajectory plan generation program
CN114728418B (en) * 2019-11-19 2023-09-12 株式会社日立制作所 Track plan generation device, track plan generation method, and recording medium
CN111256722A (en) * 2020-02-28 2020-06-09 广东博智林机器人有限公司 Path planning method, device, equipment and storage medium
CN111256722B (en) * 2020-02-28 2023-08-01 广东博智林机器人有限公司 Path planning method, device, equipment and storage medium
WO2022190535A1 (en) * 2021-03-12 2022-09-15 オムロン株式会社 Control device, rack, control system, control method, and program
WO2022196005A1 (en) * 2021-03-15 2022-09-22 オムロン株式会社 Control device, control system, control method, and program
WO2023058243A1 (en) * 2021-10-08 2023-04-13 ファナック株式会社 Control device and computer-readable recording medium storing program

Also Published As

Publication number Publication date
JP4667764B2 (en) 2011-04-13

Similar Documents

Publication Publication Date Title
JP4667764B2 (en) Route setting method
KR101798652B1 (en) Method and device to control a manipulator
EP2835228B1 (en) Robot apparatus and robot controlling method
KR101732902B1 (en) Path planning apparatus of robot and method thereof
JP4922584B2 (en) Robot system
US8219246B2 (en) System and method for controlling a robotic arm
JP6268819B2 (en) Trajectory generation method for multi-axis robot
US8055383B2 (en) Path planning device
JP5283541B2 (en) Robot motion path generation method
JP2008105132A (en) Method and apparatus for producing path of arm in joint space
US20140031982A1 (en) Robotic system and robot control device
US20110224815A1 (en) Industrial Robot And Path Planning Method For Controlling The Movement Of An Industrial Robot
JP5659890B2 (en) Robot trajectory planning system and trajectory planning method
CN114137951A (en) Online robot motion planning framework
JP2009053926A (en) Route-planning device and route-planning method
JPH01316187A (en) Method of controlling movement of joint type arm
JP2019135076A (en) Locus generation method and device
JP4304495B2 (en) Route planning method
JP7028151B2 (en) Orbit generator
JP4760732B2 (en) Route creation device
JP2007000954A (en) Robot teaching device and method
CN111283683B (en) Servo tracking accelerated convergence method for robot visual feature planning track
EP2345512A1 (en) Method of finding feasible joint trajectories for an n-dof robot with rotation invariant process (N&gt;5)
US20110264265A1 (en) Robot, control system, and recording medium for control program
WO2020161910A1 (en) Control device, control method, and recording medium

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20060830

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20080529

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20080701

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20080829

A02 Decision of refusal

Free format text: JAPANESE INTERMEDIATE CODE: A02

Effective date: 20090303

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20090422

A911 Transfer to examiner for re-examination before appeal (zenchi)

Free format text: JAPANESE INTERMEDIATE CODE: A911

Effective date: 20090508

A912 Re-examination (zenchi) completed and case transferred to appeal board

Free format text: JAPANESE INTERMEDIATE CODE: A912

Effective date: 20090529

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20101202

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

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

Free format text: PAYMENT UNTIL: 20140121

Year of fee payment: 3

R151 Written notification of patent or utility model registration

Ref document number: 4667764

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

Year of fee payment: 3

LAPS Cancellation because of no payment of annual fees