JP2012056063A - Smooth motion path generating device and smooth motion path generating method - Google Patents

Smooth motion path generating device and smooth motion path generating method Download PDF

Info

Publication number
JP2012056063A
JP2012056063A JP2010203871A JP2010203871A JP2012056063A JP 2012056063 A JP2012056063 A JP 2012056063A JP 2010203871 A JP2010203871 A JP 2010203871A JP 2010203871 A JP2010203871 A JP 2010203871A JP 2012056063 A JP2012056063 A JP 2012056063A
Authority
JP
Japan
Prior art keywords
route
intermediate point
path
point
smooth
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
JP2010203871A
Other languages
Japanese (ja)
Other versions
JP5361004B2 (en
Inventor
Keisuke Sato
圭祐 佐藤
Shinichi Takeyama
信一 竹山
Hideaki Sakai
英明 酒井
Eiji Takakura
栄次 高倉
Ryosuke Kamoi
良介 鴨井
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.)
Institute of National Colleges of Technologies Japan
Sugino Machine Ltd
Original Assignee
Institute of National Colleges of Technologies Japan
Sugino Machine Ltd
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 Institute of National Colleges of Technologies Japan, Sugino Machine Ltd filed Critical Institute of National Colleges of Technologies Japan
Priority to JP2010203871A priority Critical patent/JP5361004B2/en
Publication of JP2012056063A publication Critical patent/JP2012056063A/en
Application granted granted Critical
Publication of JP5361004B2 publication Critical patent/JP5361004B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

PROBLEM TO BE SOLVED: To eliminate unnecessary motions from a motion path and regenerate a smooth motion path.SOLUTION: A smooth motion path generating device includes: a motion path storing means; a path waypoint setting means; a path waypoint storing means; a path waypoint pair adopting means for adopting a pair of path waypoints including a path waypoint and another waypoint to be paired among waypoints; a midpoint calculating means for obtaining a midpoint between a path waypoint and another path waypoint by a calculation formula, (position data of a path waypoint+position data of another path waypoint)/2; and a midpoint interference checking means for determining whether the midpoint interferes with an obstacle. When the midpoint interferes with the obstacle, a current motion path is adopted (S7). When the midpoint does not interfere with the obstacle, the midpoint is adopted as a new path waypoint, and a smooth path generating step of selecting a new motion path from a path waypoint to another path waypoint through the new path waypoint (S6).

Description

本発明は障害物を回避するように生成された動作経路から余分な動作を削減して円滑な動作経路を再生成する円滑経路生成装置および円滑経路生成方法に関する。   The present invention relates to a smooth path generation apparatus and a smooth path generation method for regenerating a smooth operation path by reducing extra operations from an operation path generated so as to avoid an obstacle.

一般的に、原子炉など、人の進入が困難な場所において作業を行う場合、CCDカメラ、距離センサ、接触センサなどを組み込んで遠隔制御を行い、炉内の点検、形状の計測、補修作業などを行うといったマスタ・スレーブ方式が採用されるケースが多い。
特に、原子炉内において、動作経路選択を自動制御することのできる多関節ロボットを用いた作業が行われており、そうした自動制御および経路生成を実現する手法として、コンフィギュレーション空間を用いた動作経路生成法、迷路探索法、RRT(Rapidly−exploring Random Trees)法などの確率的手法を用いた動作経路生成法が開示されている(特許文献1〜3)。
In general, when working in a place where human access is difficult, such as a nuclear reactor, remote control is performed by incorporating a CCD camera, distance sensor, contact sensor, etc., inspection of the reactor, measurement of the shape, repair work, etc. In many cases, a master / slave system is used.
In particular, in a nuclear reactor, work using an articulated robot that can automatically control the operation path selection is performed. As a method for realizing such automatic control and path generation, an operation path using a configuration space is used. An operation route generation method using a probabilistic method such as a generation method, a maze search method, or an RRT (Rapidly-exploring Random Trees) method is disclosed (Patent Documents 1 to 3).

特開平4−235606号公報JP-A-4-235606 特開2008−105132号公報JP 2008-105132 A 特開2010−32326号公報JP 2010-32326 A

しかしながら、これらの動作経路計画アルゴリズムによって生成されるロボットの動作経路は、障害物の回避を優先するものであるため、その動作経路内に振動や余分な動作を含む場合があった。このため、このようにして生成された軌道でロボットを動作させると、急激な加速、減速を繰り返すためロボット本体や対象物に無理な力が加わってしまうという問題があった。   However, since the robot motion path generated by these motion path planning algorithms prioritizes the avoidance of obstacles, the motion path may include vibrations and extra motion. For this reason, when the robot is operated on the trajectory generated in this way, there is a problem that an excessive force is applied to the robot body and the target object because of rapid acceleration and deceleration.

例えば、動作経路計画アルゴリズムの中でもRRT法等の確率的手法によって比較的高速に生成されるロボットの動作経路は、確率的手法がロボットのコンフィギュレーションを断片的にしか得られないために、ランダムに選んだコンフィギュレーション点に向けて一定距離だけ進んだコンフィギュレーション点(経路途中の目標位置)を経路探索木に取り入れて探索木を成長させ、探索木に取り入れた目標位置をたどることでロボットの軌道を取得する。   For example, a robot motion path that is generated at a relatively high speed by a stochastic method such as the RRT method among motion path planning algorithms can be obtained randomly because the probabilistic method can only obtain the robot configuration in pieces. The robot trajectory is obtained by taking a configuration point (target position in the middle of the path) advanced by a certain distance toward the selected configuration point into the path search tree, growing the search tree, and following the target position incorporated in the search tree. To get.

このため、特にRRT法によって得た軌道は乱数に基づいて一定の間隔で目標位置を連結したものであり、直線的な成分が少ない断片的な折曲した軌道となるため、その動作経路内に振動や余分な動作を含む場合があり、円滑な動作経路が得られないという問題があった。   For this reason, in particular, the trajectory obtained by the RRT method is obtained by connecting the target positions at regular intervals based on random numbers and becomes a fragmented bent trajectory with few linear components. In some cases, vibration and extra motion are included, and there is a problem that a smooth motion path cannot be obtained.

本発明は、このような背景に鑑みてなされたものであり、障害物を回避するように生成された動作経路から振動や余分な動作を誘引する動作経路を削減して円滑な動作経路を再生成する円滑経路生成装置および円滑経路生成方法を提供することを課題とする。   The present invention has been made in view of such a background, and reproduces a smooth operation path by reducing an operation path that induces vibrations and extra actions from an operation path generated so as to avoid an obstacle. It is an object of the present invention to provide a smooth path generation device and a smooth path generation method.

本発明の請求項1に係る発明は、演算処理装置および記憶装置からなる制御装置を有し、初期位置から目標位置まで障害物を回避するように生成された多軸ロボットアームの動作経路を円滑化して円滑経路を再生成する円滑経路生成装置であって、前記制御装置は、前記初期位置から前記目標位置までの動作経路を格納する動作経路記憶手段と、前記動作経路における複数の経路途中点を設定する経路途中点設定手段と、前記複数の経路途中点を格納する経路途中点記憶手段と、前記多軸ロボットアームのいずれか1以上の軸について、前記経路途中点のうち対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択する経路途中点対採択手段と、この経路途中点対採択手段により採択された前記一の経路途中点と他の経路途中点との中間点を、計算式(一の経路途中点の位置データ+他の経路途中点の位置データ)/2から求める中間点演算手段と、この中間点演算手段により求めた中間点が前記障害物と干渉するかどうかを判断する中間点干渉チェック手段と、を備え、前記中間点干渉チェック手段により、前記中間点が前記障害物と干渉すると判断された場合には、当該中間点を新経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し、前記中間点干渉チェック手段により、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一の経路途中点から当該新経路途中点を通って前記他の経路途中点まで進む新動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップを実行することを特徴とする。   The invention according to claim 1 of the present invention has a control device including an arithmetic processing device and a storage device, and smoothly operates the multi-axis robot arm generated so as to avoid an obstacle from the initial position to the target position. And a smooth path generating device that regenerates a smooth path, wherein the control device stores an operation path storage unit that stores an operation path from the initial position to the target position, and a plurality of intermediate points in the operation path. A route halfway point setting means for setting a plurality of route halfway points, a route halfway point storage means for storing the plurality of route halfway points, and one or more axes of the multi-axis robot arm as a pair of the route halfway points. A route intermediate point pair adopting means for adopting a route intermediate point pair consisting of the route intermediate point of the other route and another route intermediate point, the one route intermediate point adopted by the route intermediate point pair adopting means and the other route An intermediate point calculating means for obtaining an intermediate point from the intermediate point from a calculation formula (position data of one route intermediate point + position data of another route intermediate point) / 2, and an intermediate point obtained by the intermediate point calculating means Intermediate point interference check means for determining whether or not to interfere with the obstacle, and when the intermediate point interference check means determines that the intermediate point interferes with the obstacle, the intermediate point is When a current route maintaining step of adopting the current operation route without adopting as a new route intermediate point is executed, and the intermediate point interference check means determines that the intermediate point does not interfere with the obstacle Adopts the intermediate point as a new route halfway point, changes the operation route from the one route halfway point to a new route that passes through the new route halfway point to the other route halfway point, and Reroute And executes a smooth path generating step of forming.

本発明は、前記多軸ロボットアームのいずれか1以上の軸について、対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択して、当該経路途中点対の中間点を求め、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点とする新動作経路に変更して円滑経路を再生成することで、前記一の経路途中点から前記他の経路途中点までの動作経路を円滑にすることができる。   The present invention adopts a path halfway point pair consisting of one path halfway point and another path halfway point with respect to any one or more axes of the multi-axis robot arm. By obtaining an intermediate point, and when it is determined that the intermediate point does not interfere with the obstacle, the intermediate point is changed to a new operation route that is a new route intermediate point, and a smooth route is regenerated, An operation path from one route halfway point to the other route halfway point can be made smooth.

つまり、前記中間点が前記障害物と干渉しないと判断された場合には、前記動作経路を前記一の経路途中点から当該中間点を通って前記他の経路途中点まで進む新動作経路に変更することで、前記一の経路途中点から前記他の経路途中点までの間において、迂回した経路や屈曲した経路のような振動や余分な動作を誘発する不適切な経路が含まれている場合には、迂回した経路や屈曲した経路から当該中間点を通るより直線的な経路に変更し、経路の長さを短縮して動作経路を円滑にする。   That is, when it is determined that the intermediate point does not interfere with the obstacle, the operation route is changed from the one route intermediate point to a new operation route that passes through the intermediate point to the other route intermediate point. If there is an inappropriate route that induces vibrations or extra movement, such as a detoured route or a bent route, between the midpoint of the one route and the midpoint of the other route In this case, the route is changed from a detoured route or a bent route to a more linear route passing through the intermediate point, and the length of the route is shortened to make the operation route smooth.

このようにして、本発明に係る円滑経路生成装置は、前記一の経路途中点から前記他の経路途中点までの動作経路に含まれる振動や余分な動作を誘発する不適切な動作経路をより直線的な経路に変更して円滑にすることで、多軸ロボットアームの動作における急激な加速、減速を回避してロボットアームの円滑な動作を確保することができる。   In this way, the smooth path generation device according to the present invention provides an inappropriate motion path that induces vibrations and extra motion included in the motion path from the midpoint of the one path to the midpoint of the other path. By changing to a straight path and smoothing it, it is possible to avoid a sudden acceleration and deceleration in the operation of the multi-axis robot arm and to ensure a smooth operation of the robot arm.

このため、本発明に係る円滑経路生成装置は、例えば、安全性が重視される原子力関係の機器等におけるシュラウドや複数の配管が並存するような狭あいな作業空間の中で多関節ロボットアームの動作経路を生成するような場合であっても、干渉領域を確実に回避しながら円滑な動作経路を生成することできるため、経路生成ツールとしての高い利用価値が期待される。   For this reason, the smooth path generating device according to the present invention is, for example, a multi-joint robot arm in a narrow work space in which shrouds and a plurality of pipes coexist in nuclear equipment and the like where safety is important. Even when an operation path is generated, a smooth operation path can be generated while reliably avoiding an interference region, and therefore, high utility value as a path generation tool is expected.

請求項2に係る発明は、請求項1に記載の円滑経路生成装置であって、前記経路途中点対採択手段は、前記経路途中点対を数珠つながりに連続して多数採択し、前記中間点演算手段は、前記多数の経路途中点対においてそれぞれ多数の中間点を求め、前記中間点干渉チェック手段は、前記多数の中間点がそれぞれ前記障害物と干渉するかどうかを判断して、前記円滑経路生成ステップでは、前記中間点干渉チェック手段により、前記多数の中間点のうち当該中間点がそれぞれ前記障害物と干渉すると判断された場合には、当該それぞれの中間点を新経路途中点として採択せずにそれぞれの現在のままの経路途中点対で構成される動作経路を採択する現在経路維持ステップを実行し、前記多数の中間点のうち当該中間点がそれぞれ前記障害物と干渉しないと判断された場合には、当該それぞれの中間点を新経路途中点として採択し、前記動作経路を当該それぞれの中間点に係る前記一の経路途中点から当該それぞれの新途中点を通って当該それぞれの中間点に係る前記他の経路途中点まで進む新たな動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップを実行することを特徴とする。   The invention according to claim 2 is the smooth route generation device according to claim 1, wherein the route halfway point pair adoption means adopts a number of the route halfway point pairs successively in a daisy chain, and the intermediate point The computing means obtains a large number of intermediate points in each of the numerous path halfway point pairs, and the intermediate point interference check means determines whether or not the numerous intermediate points interfere with the obstacle, respectively. In the route generation step, when the intermediate point interference check means determines that the intermediate point of the multiple intermediate points interferes with the obstacle, the intermediate point is adopted as a new route intermediate point. Without performing a current path maintenance step that adopts an operation path composed of a pair of midpoints of the path as it is without the current path. If it is determined not to interfere, the respective intermediate points are adopted as new route intermediate points, and the operation route passes from the one route intermediate point to each of the intermediate points through the respective new intermediate points. Then, a smooth path generation step is performed in which the smooth path is regenerated by changing to a new operation path that travels to the other path halfway point related to each of the intermediate points.

かかる構成によれば、前記経路途中点対採択手段により、前記経路途中点対を数珠つながりに連続して多数採択して、多数の中間点を求めることで、初期位置から目標位置まで効率よく円滑経路を再生成することができる。   According to such a configuration, the route halfway point pair selection means adopts a large number of the route halfway point pairs in a continuous manner and obtains a large number of intermediate points, thereby efficiently and smoothly from the initial position to the target position. The route can be regenerated.

請求項3に係る発明は、請求項1または請求項2に記載の円滑経路生成装置であって、前記制御装置は、前記円滑経路生成ステップの後、前記経路途中点対採択手段により、前記一の経路途中点および他の経路途中点のうち少なくとも一方が異なる一のシフト経路途中点、および他のシフト経路途中点からなる一対のシフト経路途中点対を採択するシフト経路途中点対採択ステップを実行し、前記中間点演算手段により、前記シフト経路途中点対採択手段により採択された前記一のシフト経路途中点と他のシフト経路途中点との中間点であるシフト中間点を、計算式(一のシフト経路途中点の位置データ+他のシフト経路途中点の位置データ)/2から求めるシフト中間点演算ステップを実行し、前記中間点干渉チェック手段により、前記シフト中間点が前記障害物と干渉すると判断された場合には、当該シフト中間点を新たな経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し、前記中間点干渉チェック手段により、前記シフト中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一のシフト経路途中点から当該新経路途中点を通って前記他のシフト経路途中点まで進む新動作経路に変更して前記円滑経路を再生成するシフト円滑経路生成ステップを実行し、このようにして、前記円滑経路生成ステップの後に前記現在経路維持ステップまたは前記シフト円滑経路生成ステップを実行し、前記円滑経路生成ステップから前記現在経路維持ステップまたは前記シフト円滑経路生成ステップを順次繰り返して動作経路を円滑化することを特徴とする。   The invention according to claim 3 is the smooth path generation apparatus according to claim 1 or 2, wherein the control apparatus performs the path halfway point selection means after the smooth path generation step by the path halfway point selection means. A step of adopting a shift path halfway point pair step of adopting a pair of shift path halfway points consisting of one shift path halfway point and at least one of the other route halfway points and the other shift route halfway point And a shift intermediate point that is an intermediate point between the one shift path intermediate point and the other shift path intermediate point selected by the shift path intermediate point pair selecting unit is calculated by the calculation formula ( (Shift position midpoint position data + position data of another shift path midpoint) / 2) is executed, and the midpoint interference check means performs the shift point calculation step. If it is determined that the intermediate point is interfered with the obstacle, a current route maintaining step of adopting the current operation route without adopting the shift intermediate point as a new route intermediate point is executed, When the intermediate point interference check means determines that the shift intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is determined from the one shift route intermediate point. A shift smooth path generation step is performed in which the smooth path is regenerated by changing to a new operation path that passes through the new path midpoint to the other shift path midpoint, and thus the smooth path generation step The current path maintaining step or the shift smooth path generating step is executed after the current path maintaining step or the shift smooth path from the smooth path generating step. Characterized by smooth the motion path sequentially repeated production steps.

かかる構成によれば、前記一の経路途中点および他の経路途中点のうち少なくとも一方が異なる一のシフト経路途中点、および他のシフト経路途中点からなる一対のシフト経路途中点対を採択して、当該シフト経路途中点対の中間点を求めることで、経路途中点対からシフト経路途中点対に採択する経路途中点をずらしながら中間点を求める。
そして、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点とする新動作経路に変更して円滑経路を再生成することで、前記一のシフト経路途中点から前記他のシフト経路途中点までの動作経路を円滑にすることができる。
According to such a configuration, one shift path midpoint and at least one of the one path midpoint and the other path midpoint are different, and a pair of shift path midpoint pairs including another shift path midpoint is adopted. Thus, by determining the intermediate point of the shift path intermediate point pair, the intermediate point is determined while shifting the path intermediate point to be adopted as the shift path intermediate point pair from the path intermediate point pair.
When it is determined that the intermediate point does not interfere with the obstacle, the one shift is performed by changing the intermediate point to a new operation route having a new route halfway and regenerating a smooth route. The operation path from the midway point of the route to the midway point of the other shift route can be made smooth.

請求項4に係る発明は、初期位置から目標位置まで障害物を回避するように生成された多軸ロボットアームの動作経路を円滑化して円滑経路を再生成する円滑経路生成方法であって、演算処理装置および記憶装置からなる制御装置を有する円滑経路生成装置を使用して、前記演算処理装置を構成する経路途中点設定手段により、前記動作経路における複数の経路途中点を設定する経路途中点設定ステップと、前記演算処理装置を構成する経路途中点採択手段により、前記多軸ロボットアームのいずれか1以上の軸について、前記経路途中点のうち対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択する経路途中点対採択ステップと、前記演算処理装置を構成する中間点演算手段により、前記経路途中点対採択ステップで採択された前記一の経路途中点と他の経路途中点との中間点を、計算式(一の経路途中点の位置データ+他の経路途中点の位置データ)/2から求める中間点演算ステップと、前記演算処理装置を構成する中間点干渉チェック手段により、前記中間点演算ステップで求めた中間点が前記障害物と干渉するかどうかを判断する中間点干渉チェックステップと、を含み、この中間点干渉チェックステップにより、前記中間点が前記障害物と干渉すると判断された場合には、当該中間点を新経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップと、前記中間点干渉チェックステップにより、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一の経路途中点から当該新経路途中点を通って前記他の経路途中点まで進む新動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップを実行することを特徴とする。   The invention according to claim 4 is a smooth path generation method for regenerating a smooth path by smoothing an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position. A route halfway point setting in which a plurality of route halfway points in the operation route are set by a route halfway point setting means constituting the arithmetic processing unit using a smooth route generation device having a control device comprising a processing device and a storage device. One of the path halfway points and another path for any one or more axes of the multi-axis robot arm by the step and path halfway point adopting means constituting the arithmetic processing unit; A route halfway point pair adopting step that adopts a route halfway point pair consisting of halfway points, and a halfway point pair adopting step that constitutes the arithmetic processing unit. Intermediate point calculation for obtaining an intermediate point between the one route intermediate point and the other route intermediate point adopted in step 2 from a calculation formula (position data of one route intermediate point + position data of another route intermediate point) / 2. And an intermediate point interference check step for determining whether or not the intermediate point obtained in the intermediate point calculation step interferes with the obstacle by the intermediate point interference check means constituting the arithmetic processing unit. If the intermediate point interference check step determines that the intermediate point interferes with the obstacle, the current route maintaining step of adopting the current operation route without adopting the intermediate point as a new route intermediate point When the intermediate point interference check step determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is set as the previous route. And executes a smooth path generating step of regenerating the smooth path is changed to the new operation route proceeds from a path midway point to the other path midway point through the new path midway point.

かかる構成によれば、前記多軸ロボットアームのいずれか1以上の軸について、対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択して、当該経路途中点対の中間点を求め、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点とする新動作経路に変更して円滑経路を再生成する円滑経路生成ステップを実行することで、前記一の経路途中点から前記他の経路途中点までの動作経路を円滑にすることができる。   According to such a configuration, for any one or more axes of the multi-axis robot arm, a pair of route intermediate points consisting of one route intermediate point and another route intermediate point are adopted, and the route intermediate point is selected. A smooth route that obtains a midpoint of a pair and, when it is determined that the midpoint does not interfere with the obstacle, changes the midpoint to a new operation route with a new route halfway and regenerates a smooth route By executing the generating step, an operation path from the one way intermediate point to the other route intermediate point can be made smooth.

本発明によれば、障害物を回避するように生成された動作経路から振動や余分な動作を誘引する動作経路を削減して円滑な動作経路を再生成する円滑経路生成装置および円滑経路生成方法を提供することができる。したがって、本発明は、自由度が高い多関節ロボットであっても円滑に適用できることから、原子力分野等における狭あいな作業空間の中で複雑な動作をさせたい場合等において特に好適である。   According to the present invention, a smooth path generating device and a smooth path generating method for regenerating a smooth motion path by reducing motion paths that induce vibrations and extra motion from motion paths generated so as to avoid obstacles. Can be provided. Therefore, since the present invention can be applied smoothly even to an articulated robot having a high degree of freedom, it is particularly suitable when it is desired to perform a complex operation in a narrow work space in the nuclear power field or the like.

ロボットアームにおける6軸構成を示す斜視図である。It is a perspective view which shows 6 axis | shaft structure in a robot arm. 2軸のロボットアームにおけるロボットアームの姿勢と障害物との関係を示す図であり、(a)は実空間における関係、(b)はコンフィギュレーション空間における関係を示す。It is a figure which shows the relationship between the attitude | position of a robot arm in a biaxial robot arm, and an obstruction, (a) shows the relationship in real space, (b) shows the relationship in configuration space. 本発明の実施形態に係る円滑経路生成装置の構成を示すブロック図である。It is a block diagram which shows the structure of the smooth path | route production | generation apparatus which concerns on embodiment of this invention. 本発明の実施形態に係る円滑経路生成装置の動作を説明するための図であり、(a)は円滑化する前の動作経路、(b)は(c,1)から(e,4)までの経路を対象とする円滑経路生成ステップ、(c)は(e,4)から(h,7)までの経路を対象とする現在経路維持ステップを示す。It is a figure for demonstrating operation | movement of the smooth path | route production | generation apparatus which concerns on embodiment of this invention, (a) is an operation | movement path | route before smoothing, (b) is from (c, 1) to (e, 4). (C) shows a current route maintenance step for the routes from (e, 4) to (h, 7). 本発明の実施形態に係る円滑経路生成装置の動作を説明するための図であり、(a)は(h,7)から(i,9)までの経路を対象とする円滑経路生成ステップ、(b)はシフト円滑経路生成ステップ、(c)は円滑経路生成ステップおよびシフト円滑経路生成ステップにより再生成された第1次円滑経路を示す。It is a figure for demonstrating operation | movement of the smooth path | route production | generation apparatus which concerns on embodiment of this invention, (a) is the smooth path | route production | generation step for the path | route from (h, 7) to (i, 9), ( b) shows the shift smooth path generation step, and (c) shows the first smooth path regenerated by the smooth path generation step and the shift smooth path generation step. 本発明の実施形態に係る経路生成装置の動作を説明するための流れ図である。It is a flowchart for demonstrating operation | movement of the route generation apparatus which concerns on embodiment of this invention. 本発明に係る円滑経路生成装置における第1の実施例の動作を説明するための図である。It is a figure for demonstrating operation | movement of the 1st Example in the smooth path | route production | generation apparatus which concerns on this invention. 本発明に係る円滑経路生成装置における第2の実施例の動作を説明するための図である。It is a figure for demonstrating operation | movement of the 2nd Example in the smooth path | route production | generation apparatus which concerns on this invention. ロボットアームの初期位置から目標位置までの動作のイメージを示す斜視図であり、(a)は初期位置、(b)は目標位置、(c)は初期位置から目標位置までの動作のイメージを示す。It is a perspective view which shows the image of operation | movement from the initial position of a robot arm to a target position, (a) is an initial position, (b) is a target position, (c) shows the image of operation | movement from an initial position to a target position. .

本発明の実施形態に係る円滑経路生成装置1(図3参照)について、適宜図を参照しながら詳細に説明する。円滑経路生成装置1は、多軸ロボットアームである6軸を有するロボットアーム100(図1参照)において障害物を回避するように予め生成された動作経路(図4A(a)に示す動作経路)を円滑して円滑動作経路(図4B(a)に示す動作経路)を再生成する場合を例として説明する。   A smooth path generating apparatus 1 (see FIG. 3) according to an embodiment of the present invention will be described in detail with reference to the drawings as appropriate. The smooth path generation device 1 is an action path (an action path shown in FIG. 4A) generated in advance so as to avoid an obstacle in a robot arm 100 (see FIG. 1) having six axes that is a multi-axis robot arm. An example will be described in which the smooth operation path (the operation path shown in FIG. 4B (a)) is regenerated.

なお、図4Aないし図4B、および図6、図7においては、コンフィギュレーション空間(Cスペース)を用いて、動作経路を円滑にする手法について説明するが、Cスペース上で生成された動作経路に限定されるものではなく、実空間上で適用することも可能である。   4A to 4B, and FIGS. 6 and 7, a method for smoothing the operation path using the configuration space (C space) will be described. However, the operation path generated on the C space is described as an example. The present invention is not limited, and application in real space is also possible.

また、ロボットアーム100(図1参照)において予め生成された動作経路(図4A(a)に示す動作経路)は、コンフィギュレーション空間を用いた動作経路生成法により生成されたものを例として説明するが、迷路探索法やRRT法などの確率的手法を用いて生成されたものであってもよく動作経路生成手法には限定されないため詳細な説明は省略する。   In addition, the motion path (the motion path shown in FIG. 4A) generated in advance in the robot arm 100 (see FIG. 1) will be described as an example generated by the motion path generation method using the configuration space. However, it may be generated using a probabilistic method such as a maze search method or an RRT method, and is not limited to the operation route generation method, and thus detailed description thereof is omitted.

コンフィギュレーション空間(Cスペース)は、マニュピレータ(ロボットアーム)の自由度の数と同数の次元数を持つ仮想空間であり、このCスペース上では、ロボットアームのコンフィギュレーション(姿勢)は、ロボットアームの自由度を表すパラメータを座標軸の値とした場合、点(コンフィギュレーション点)で表すことができる。
例えば、ロボットアームのすべてのコンフィギュレーションについて障害物との干渉の有無を調査してロボットアームが障害物と干渉しない空間(自由空間)をCスペース上に構築し、その中でロボットアームの動作経路を生成する。
The configuration space (C space) is a virtual space having the same number of dimensions as the number of degrees of freedom of the manipulator (robot arm). On this C space, the configuration (posture) of the robot arm is the robot arm configuration. When the parameter representing the degree of freedom is a coordinate axis value, it can be represented by a point (configuration point).
For example, for all configurations of the robot arm, the presence or absence of interference with the obstacle is investigated, and a space (free space) in which the robot arm does not interfere with the obstacle is constructed on the C space, and the movement path of the robot arm in that space Is generated.

ロボットアーム100は、図1に示すように、6つの関節(6つの自由度)からなる6軸J1,J2,J3,J4,J5,J6を備えている。
ロボットアームの自由度を表すパラメータを関節角度として、ロボットアーム100の6軸J1,J2,J3,J4,J5,J6の関節角度をそれぞれθ1,θ2,θ3,θ4,θ5,θ6とすれば、ロボットアーム100のコンフィギュレーション(姿勢)はθ1,θ2,θ3,θ4,θ5,θ6で表すことができる。
なお、ロボットアーム100は、一般的にはロボットアーム自体が移動するような移動体として広く観念されるものであればよく、特に限定して解釈されるものではない。
As shown in FIG. 1, the robot arm 100 includes six axes J1, J2, J3, J4, J5, and J6 including six joints (six degrees of freedom).
Assuming that a parameter representing the degree of freedom of the robot arm is a joint angle and the joint angles of the six axes J1, J2, J3, J4, J5, and J6 of the robot arm 100 are θ1, θ2, θ3, θ4, θ5, and θ6, The configuration (posture) of the robot arm 100 can be represented by θ1, θ2, θ3, θ4, θ5, and θ6.
In general, the robot arm 100 is not particularly limited as long as it is widely considered as a moving body in which the robot arm itself moves.

このように、ロボットアーム100の自由度を表すパラメータ(例えば、関節角度や伸縮長さ)を座標軸の値とした場合、ロボットアーム100のコンフィギュレーション点はCスペース上の点として表現することができる。   As described above, when the parameters representing the degree of freedom of the robot arm 100 (for example, the joint angle and the expansion / contraction length) are coordinate axis values, the configuration points of the robot arm 100 can be expressed as points on the C space. .

例えば、図2に示すように、簡略化のために例えば、実空間において関節角度をθxとθyで表すことができる2軸(θx,θy)のロボットアーム100′(図2(a)参照)について、横軸にθxをとり縦軸にθyをとれば、Cスペース上において、ロボットアーム100′のコンフィギュレーション(姿勢)をコンフィギュレーション点(θx,θy)して表すことができる。   For example, as shown in FIG. 2, for simplification, for example, a two-axis (θx, θy) robot arm 100 ′ (see FIG. 2A) in which joint angles can be expressed by θx and θy in real space. If the horizontal axis represents θx and the vertical axis represents θy, the configuration (posture) of the robot arm 100 ′ can be represented as configuration points (θx, θy) on the C space.

一方、ロボットアーム100′が作業空間中の障害物Ob(Obstacle)と干渉(衝突)するコンフィギュレーション点を干渉点と定義する。干渉点が単独で存在することはまれであり、一般に連続した点の集合である干渉領域CA(Collision Area)としてCスペース上に表すことができる(図2(b)参照)。
このようにして、図2(b)に示すように、Cスペース上において、ロボットアーム100′のコンフィギュレーションと干渉領域CAを表すことで、動作経路におけるロボットアームと障害物との干渉をチェックすることができる。
On the other hand, a configuration point where the robot arm 100 ′ interferes (collises) with an obstacle Ob (Obscale) in the work space is defined as an interference point. Interference points are rarely present alone, and can generally be represented on the C space as an interference area CA (Collision Area) which is a set of continuous points (see FIG. 2B).
In this way, as shown in FIG. 2B, the interference between the robot arm and the obstacle in the operation path is checked by representing the configuration of the robot arm 100 ′ and the interference area CA on the C space. be able to.

なお、図2では2軸(θx,θy)について説明したが、計算量が異なるとしても同じ考え方でロボットアーム100(図1)のような6軸(θ1,θ2,θ3,θ4,θ5,θ6)に発展させて処理することも可能である。このため、図4Aや図4Bに示すように、2軸のみを平面的に示す方が理解しやすいので、便宜上簡略化したものである。   In FIG. 2, the two axes (θx, θy) have been described. However, the six axes (θ1, θ2, θ3, θ4, θ5, θ6) like the robot arm 100 (FIG. 1) can be used in the same way even if the calculation amount is different. It is also possible to develop and process. For this reason, as shown in FIG. 4A and FIG. 4B, it is easier to understand if only two axes are shown in a plan view, and therefore it is simplified for convenience.

円滑経路生成装置1は、図3と図4Aおよび図4Bに示すように、初期位置である初期コンフィギュレーション点qiから目標位置である目標コンフィギュレーション点qgまで障害物を回避するように生成されたロボットアーム100の動作経路(「円滑処理前の動作経路」という。図4A(a)参照)を円滑化して円滑経路(図4B(c)参照)を再生成する装置である(図3、図8参照)。
ここで、Cスペースを用いた動作計画とは、図4Aと図4Bに示すように、Cスペース上において干渉領域CAを避けて初期コンフィギュレーション点qi(initial)と目標コンフィギュレーション点qg(goal)を接続する経路を生成し、実空間における経路に置き換えて動作経路を探索する手法である。
As shown in FIGS. 3, 4A, and 4B, the smooth path generation device 1 is generated so as to avoid an obstacle from an initial configuration point qi that is an initial position to a target configuration point qg that is a target position. This is a device that smoothes the motion path of the robot arm 100 (referred to as “motion path before smooth processing”, see FIG. 4A (a)) and regenerates the smooth path (see FIG. 4B (c)) (FIG. 3, FIG. 8).
Here, as shown in FIG. 4A and FIG. 4B, the operation plan using the C space is an initial configuration point qi (initial) and a target configuration point qg (goal) avoiding the interference area CA on the C space. This is a method of generating a route connecting the, and searching for an operation route by replacing it with a route in the real space.

円滑経路生成装置1は、図3に示すように、作業者(不図示)がデータ入力等を行う操作盤2と、図示しないCPU等からなる演算処理装置3と、種々のデータを格納する記憶装置4と、を備えている。
操作盤2には、ディスプレイ21が配設され、ロボットアーム100や作業空間を3D(3次元)グラフィックス表示して、作業者(不図示)が生成された動作経路を視認できる。
As shown in FIG. 3, the smooth path generating device 1 includes an operation panel 2 on which an operator (not shown) inputs data, an arithmetic processing device 3 including a CPU (not shown), and a memory for storing various data. And a device 4.
The operation panel 2 is provided with a display 21, and the robot arm 100 and the work space can be displayed in 3D (three-dimensional) graphics so that an operation path generated by an operator (not shown) can be viewed.

演算処理装置3は、図3に示すように、経路途中点設定手段31と、経路途中点対採択手段32と、中間点演算手段33と、中間点干渉チェック手段34と、を備えている。   As shown in FIG. 3, the arithmetic processing device 3 includes a route halfway point setting unit 31, a route halfway point pair selection unit 32, a middle point computation unit 33, and a middle point interference check unit 34.

記憶装置4は、初期位置記憶手段41と、目標位置記憶手段42と、ロボット形状記憶手段43と、作業空間記憶手段44と、動作経路記憶手段45と、経路途中点記憶手段46と、を備えている。   The storage device 4 includes an initial position storage unit 41, a target position storage unit 42, a robot shape storage unit 43, a work space storage unit 44, an operation path storage unit 45, and a path halfway point storage unit 46. ing.

以下、円滑経路生成装置1を構成する具体的手段、およびその作用効果について、主として図3と図4Aないし図4Bを参照しながら円滑経路生成装置1の動作の順に沿って説明する。
参照する図4Aないし図4Bにおいて、細線で表示された図の縦方向のグリッド線をa〜kまで、横方向のグリッド線を1〜10まで符号をつけて示し、それらのグリッド線の交点を格子点といい(a,k)〜(l,10)のように表示する。ただし、グリッド線の間隔等の表示は模式的なものであり縦横の比率等は簡略化したものである。
Hereinafter, specific means constituting the smooth path generation device 1 and its operation and effects will be described along the order of operations of the smooth path generation device 1 mainly with reference to FIGS. 3 and 4A to 4B.
In FIG. 4A to FIG. 4B to be referred to, the vertical grid lines of the figures displayed as thin lines are indicated by a to k, the horizontal grid lines are indicated by 1 to 10, and the intersections of these grid lines are indicated. It is called a grid point and is displayed as (a, k) to (l, 10). However, the display of the grid line spacing and the like is schematic, and the aspect ratio and the like are simplified.

格子点は、Cスペースを細かく分割して、離散化されたコンフィギュレーション点の集合として扱うための概念である。このように、Cスペースを連続した空間として認識するのではなく、格子点を基準として離散化(量子化)し、この量子化された格子点を通るように新コンフィギュレーション点を採択しながら動作経路を生成することで、Cスペース上の演算処理を簡略化することができる。
例えば、ロボットアーム100(図1参照)の根元部分の関節角度θ1〜θ3を1度刻みにして、手先部分の関節角度θ4〜θ6を5度刻みにしてCスペースにおける格子点を設定し、この格子点を基準にして離散化されたCスペースを構築することができる。
The lattice point is a concept for finely dividing the C space and handling it as a set of discretized configuration points. In this way, the C space is not recognized as a continuous space, but is discretized (quantized) with reference to the lattice point, and the new configuration point is adopted so as to pass through the quantized lattice point. By generating the route, the arithmetic processing on the C space can be simplified.
For example, the joint angles θ1 to θ3 of the base portion of the robot arm 100 (see FIG. 1) are set in increments of 1 and the joint angles θ4 to θ6 of the hand portion are set in increments of 5 degrees to set lattice points in the C space. A discretized C space can be constructed with reference to the grid points.

初期位置記憶手段41は、図3および図4A(a)に示すように、初期コンフィギュレーション点qiであるロボットアーム100の初期姿勢(実空間では図8(a)参照)を各関節の関節角度等により記憶する装置であり、操作盤2から作業者(不図示)が入力したり、演算処理装置3により初期位置を角度センサ等の検出器で検出して初期コンフィギュレーション点qiとして設定したりすることができる。
例えば、初期コンフィギュレーション点qiは、qi=(θ1,θ2,θ3,θ4,θ5,θ6)=(−170,90,0,25、−85,75)のように関節角度をパラメータとして表すことができる。
As shown in FIG. 3 and FIG. 4A (a), the initial position storage means 41 uses the initial posture of the robot arm 100 that is the initial configuration point qi (see FIG. 8A in real space) as the joint angle of each joint. Or the like, and an operator (not shown) inputs from the operation panel 2, or the arithmetic processing unit 3 detects the initial position with a detector such as an angle sensor and sets it as the initial configuration point qi. can do.
For example, the initial configuration point qi represents the joint angle as a parameter such that qi = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 170, 90, 0, 25, −85, 75). Can do.

目標位置記憶手段42は、目標コンフィギュレーション点qgであるロボットアーム100の目標姿勢(実空間では図8(b)参照)を各関節の関節角度等により記憶する手段であり、操作盤2から作業者(不図示)が入力したり、演算処理装置3により目標位置を角度センサ等の検出器で検出して目標コンフィギュレーション点qgとして設定したりすることができる。
例えば、目標コンフィギュレーション点qgは、qg=(θ1,θ2,θ3,θ4,θ5,θ6)=(−71,−75,38,80,−10,25)のように関節角度をパラメータとして表すことができる。
The target position storage means 42 is a means for storing the target posture of the robot arm 100 (see FIG. 8B in the real space) that is the target configuration point qg based on the joint angle of each joint. A target position (not shown) can be input, or the target position can be detected by the arithmetic processing unit 3 with a detector such as an angle sensor and set as the target configuration point qg.
For example, the target configuration point qg represents the joint angle as a parameter such that qg = (θ1, θ2, θ3, θ4, θ5, θ6) = (− 71, −75, 38, 80, −10, 25). be able to.

ロボット形状記憶手段43は、CADファイル等から作成したロボットアーム100の形状データを格納するが、データ処理を簡略化するため、例えば、ロボットアーム100の形状データをポリゴンと呼ばれる多角形等の最小構成単位(例えば三角形)の集合で表して、この多角形の集合をCスペース上の形状として表現して記憶することができる。
かかる構成により、実空間におけるロボットアーム100の形状データを三角形等のポリゴンの集合として表して、Cスペース上の形状として認識することで、Cスペース上におけるロボットアーム100の形状データを簡略化(離散化)して演算処理を短縮化することができる。
The robot shape storage means 43 stores the shape data of the robot arm 100 created from a CAD file or the like, but in order to simplify the data processing, for example, the shape data of the robot arm 100 is a minimum configuration such as a polygon called a polygon. Expressed as a set of units (for example, triangles), this polygon set can be expressed and stored as a shape on the C space.
With this configuration, the shape data of the robot arm 100 in the C space is simplified (discrete) by representing the shape data of the robot arm 100 in the real space as a set of polygons such as triangles and recognizing the shape as the shape on the C space. Calculation processing can be shortened.

作業空間記憶手段44は、障害物Ob(図8参照)が配置された作業空間の形状データを格納するが、データ処理を簡略化するため、例えば、障害物Obの形状データをポリゴン(例えば三角形等の多角形)の集合で表して、この三角形等の集合をCスペース上の形状として表現して記憶することができる。
かかる構成により、Cスペース上における作業空間の形状データを簡略化(離散化)して演算処理を短縮化することができる。
The work space storage means 44 stores the shape data of the work space in which the obstacle Ob (see FIG. 8) is arranged. In order to simplify the data processing, for example, the shape data of the obstacle Ob is converted to a polygon (for example, a triangle). The set of triangles and the like can be expressed and stored as a shape on the C space.
With this configuration, it is possible to simplify (discretize) the shape data of the work space on the C space and shorten the arithmetic processing.

動作経路記憶手段45は、図3および図4A(a)に示すように初期位置である初期コンフィギュレーション点qiから目標位置である目標コンフィギュレーション点qgまでの動作経路を格納する記憶装置である。具体的には、動作経路記憶手段45は、動作経路を構成するコンフィギュレーション点を格納し、新たな動作経路が生成された場合には、その都度更新される。   The operation path storage means 45 is a storage device that stores an operation path from an initial configuration point qi that is an initial position to a target configuration point qg that is a target position, as shown in FIGS. 3 and 4A (a). Specifically, the operation path storage unit 45 stores configuration points constituting the operation path, and is updated each time a new operation path is generated.

例えば、動作経路記憶手段45は、図4A(a)に示す状態における円滑化される前の動作経路の場合には、初期コンフィギュレーション点qi(c,1)からコンフィギュレーション点(e,3),(e,4),(h,4),(h,7),(i,8)を通って、目標コンフィギュレーション点qg(i,9)までを構成する各コンフィギュレーション点を格納して動作経路を記憶している。   For example, in the case of the operation path before being smoothed in the state shown in FIG. 4A (a), the operation path storage means 45 changes from the initial configuration point qi (c, 1) to the configuration point (e, 3). , (E, 4), (h, 4), (h, 7), (i, 8) to store the configuration points constituting the target configuration point qg (i, 9) The operation path is stored.

このように、動作経路は、初期コンフィギュレーション点qi(c,1)からコンフィギュレーション点(e,3)まで、およびコンフィギュレーション点(e,3)からコンフィギュレーション点(e,4)までのような直線経路と、方向の異なる直線経路を連結するコンフィギュレーション点(e,4)となる折曲点と、を備えて生成されている。   Thus, the operation path is from the initial configuration point qi (c, 1) to the configuration point (e, 3) and from the configuration point (e, 3) to the configuration point (e, 4). And a bending point that is a configuration point (e, 4) that connects straight paths having different directions.

経路途中点設定手段31は、図3および図4A(a)に示すように、動作経路における複数の経路途中点を設定する演算装置である。ここで、説明の便宜上簡略化して「経路途中点」という名称を使用するが初期位置qiおよび目標位置qgを排除する趣旨ではない。   The route halfway point setting means 31 is a computing device that sets a plurality of route halfway points in the operation route, as shown in FIGS. 3 and 4A (a). Here, for convenience of explanation, the name “route halfway point” is used for simplification, but this is not intended to exclude the initial position qi and the target position qg.

例えば、図4A(a)に示す動作経路において、経路途中点設定手段31は、複数の経路途中点として、初期コンフィギュレーション点qi(c,1)および目標コンフィギュレーション点qg(i,9)の他、動作経路における折曲点であるコンフィギュレーション点(e,3),(e,4),(h,4),(h,7),(i,8)を採択して設定している。   For example, in the operation route shown in FIG. 4A (a), the route halfway point setting means 31 sets the initial configuration point qi (c, 1) and the target configuration point qg (i, 9) as a plurality of route halfway points. In addition, configuration points (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) that are bending points in the operation path are adopted and set. .

経路途中点記憶手段46は、経路途中点設定手段31により設定された複数の経路途中点を格納する記憶装置であり、具体的には、図4A(a)において、初期コンフィギュレーション点qi(c,1)および目標コンフィギュレーション点qg(i,9)、経路途中点である折曲点(e,3),(e,4),(h,4),(h,7),(i,8)を格納している。   The route halfway storage means 46 is a storage device that stores a plurality of route halfway points set by the route halfway point setting means 31. Specifically, in FIG. 4A (a), the initial configuration point qi (c , 1) and the target configuration point qg (i, 9), the bending point (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) is stored.

経路途中点対採択手段32は、図4A(b)と(c)に示すように、ロボットアーム100(図1参照)のいずれか1以上の軸について、経路途中点設定手段31により設定された複数の経路途中点のうち対となる一の経路途中点、および他の経路途中点からなる経路途中点対q1,q2(図4A(b))、経路途中点対q2,q3(図4A(c))、および経路途中点対q3,q4(図4B(a))を採択する演算装置である。   As shown in FIGS. 4A and 4C, the route halfway point selection means 32 is set by the route halfway point setting means 31 for any one or more axes of the robot arm 100 (see FIG. 1). Among a plurality of route intermediate points, a pair of route intermediate points and another route intermediate point pair q1, q2 (FIG. 4A (b)) and route intermediate point pairs q2, q3 (FIG. 4A ( c)), and a midway point pair q3, q4 (FIG. 4B (a)).

ここで、経路途中点対採択手段32による経路途中点対の採択の仕方は、動作経路の形態や目的に応じて適宜設定するが、例えば、経路途中点対採択手段32は、経路途中点対が数珠つながりに連続するように、折曲点を3箇所ずつ連続して選定し、選定した3箇所の折曲点において、両側の折曲点のうちの一方を一の経路途中点とし、他方を他の経路途中点とすることができる。このようにして経路途中点を設定すると、一の経路途中点と他の経路途中点の間には折曲点qt(図4A(b)のqt1,図4A(c)のqt2、図4B(a)のqt3)が経路途中点対の間に挟まれて存在するため、この折曲点qtを円滑処理の対象とすることができる。   Here, the way of selecting the route intermediate point pair by the route intermediate point pair selecting unit 32 is appropriately set according to the form and purpose of the operation route. For example, the route intermediate point pair selecting unit 32 may select the route intermediate point pair. Is selected continuously in three places, so that one of the bent points on both sides is one route halfway point, and the other Can be the other waypoint. When the route halfway point is set in this manner, a bending point qt (qt1 in FIG. 4A (b), qt2 in FIG. 4A (c), FIG. 4B (b) between one route halfway point and another route halfway point. Since qt3) of a) is sandwiched between a pair of intermediate points on the route, this bending point qt can be the target of smooth processing.

つまり、図4A(a)において、初期コンフィギュレーション点qi(c,1)と折曲点(e,3),(e,4)の3箇所の経路途中点と、折曲点(e,4)を含んで連続する折曲点(h,4),(h,7)の3箇所の経路途中点と、折曲点(h,7)を含んで連続する折曲点(i,8)および目標コンフィギュレーション点qg(i,9)の3箇所の経路途中点と、からなる3箇所ずつの数珠つながりに連続する3組の経路途中点を選定する。   In other words, in FIG. 4A (a), three route intermediate points of the initial configuration point qi (c, 1) and the bending points (e, 3), (e, 4) and the bending point (e, 4). ) Including continuous bending points (h, 4), (h, 7), and three continuous path points including bending points (h, 7). In addition, three sets of route halfway points that are connected in a sequence of three points each consisting of three route halfway points of the target configuration point qg (i, 9) are selected.

そして、それぞれの組の両側の途中点である初期コンフィギュレーション点qi(c,1)と折曲点(e,4)が図4A(b)の経路途中点対q1,q2を構成し、折曲点(e,4)と折曲点(h,7)が図4A(c)の経路途中点対q2,q3を構成し、折曲点(h,7)と目標コンフィギュレーション点qg(i,9)が図4B(a)の経路途中点対q3,q4をそれぞれ構成する。   Then, the initial configuration points qi (c, 1) and the bending points (e, 4), which are halfway points on both sides of each set, constitute the route halfway point pair q1, q2 in FIG. 4A (b). The inflection point (e, 4) and the inflection point (h, 7) constitute the path intermediate point pair q2 and q3 in FIG. 4A (c), and the inflection point (h, 7) and the target configuration point qg (i , 9) constitute the path halfway point pairs q3, q4 in FIG. 4B (a), respectively.

このとき、経路途中点対q1,q2の間にはさまれて折曲点qt1(図4A(b))が存在し、経路途中点対q2,q3の間にはさまれて折曲点qt2(図4A(c))が存在し、経路途中点対q3,q4の間にはさまれて折曲点qt3(図4B(a))が存在する。   At this time, there is a bending point qt1 (FIG. 4A (b)) between the route halfway point pair q1, q2, and between the route halfway point pair q2, q3, the bending point qt2. (FIG. 4A (c)) exists, and a bending point qt3 (FIG. 4B (a)) exists between the pair of intermediate points q3 and q4.

中間点演算手段33は、経路途中点対採択手段32により採択された経路途中点を構成する一の経路途中点と他の経路途中点との中間点を、計算式(一の経路途中点の位置データ+他の経路途中点の位置データ)/2から求める演算装置である。
具体的には、図4A(b)に示すように、経路途中点対q1,q2として初期コンフィギュレーション点qi(c,1)と折曲点(e,4)を採択した場合には、これらの中間点qm1は、
q1=(θ11,θ12,θ13,θ14,θ15,θ16)
q2=(θ21,θ22,θ23,θ24,θ25,θ26)
とし、6軸を対象として中間点qm1を求める場合には、
qm1=(θm11,θm12,θm13,θm14,θm15,θm16)
とすると、
θm11=(θ11+θ21)/2,
θm12=(θ12+θ22)/2,
θm13=(θ13+θ23)/2,
θm14=(θ14+θ24)/2,
θm15=(θ15+θ25)/2,
θm16=(θ16+θ26)/2
から求められる。中間点qm2,qm3においても同様である。
The intermediate point calculation means 33 calculates an intermediate point between one route intermediate point and another route intermediate point constituting the route intermediate point adopted by the route intermediate point pair selection means 32 by a calculation formula (one route intermediate point This is a calculation device obtained from (position data + position data of other route halfway point) / 2.
Specifically, as shown in FIG. 4A (b), when the initial configuration point qi (c, 1) and the bending point (e, 4) are adopted as the route intermediate point pair q1, q2, The intermediate point qm1 of
q1 = (θ11, θ12, θ13, θ14, θ15, θ16)
q2 = (θ21, θ22, θ23, θ24, θ25, θ26)
And when obtaining the midpoint qm1 for the six axes,
qm1 = (θm11, θm12, θm13, θm14, θm15, θm16)
Then,
θm11 = (θ11 + θ21) / 2
θm12 = (θ12 + θ22) / 2,
θm13 = (θ13 + θ23) / 2
θm14 = (θ14 + θ24) / 2
θm15 = (θ15 + θ25) / 2
θm16 = (θ16 + θ26) / 2
It is requested from. The same applies to the intermediate points qm2 and qm3.

なお、本実施形態においては、ロボットアーム100(図1参照)が有する6軸のすべてを対象として中間点を求めたが、これに限定されるものではなく、6軸のうち1以上の軸について中間点を求めてもよい。6軸のうちのどの軸を対象とするかは、例えば、動作経路の形態や目的に応じて適宜定められるものであるが、例えば分散化の幅(グリッドの間隔)が大きいものを選択したり、障害物Ob(図8参照)の配置や各軸の可動範囲等を考慮したりすることができる。   In the present embodiment, the intermediate points are obtained for all six axes of the robot arm 100 (see FIG. 1), but the present invention is not limited to this, and one or more of the six axes are included. An intermediate point may be obtained. Which of the six axes is the target is determined as appropriate according to, for example, the form and purpose of the operation path. For example, the one having a large dispersion width (grid spacing) can be selected. The arrangement of the obstacle Ob (see FIG. 8), the movable range of each axis, and the like can be taken into consideration.

中間点干渉チェック手段34は、図3と図4Aに示すように、中間点演算手段33により求めた中間点が障害物Obと干渉するかどうかを判断する演算装置である。
つまり、中間点干渉チェック手段34は、対象となる中間点におけるロボットアーム100(図1参照)の関節角度(θ1,θ2,θ3,θ4,θ5,θ6)から、ロボット形状記憶手段43に格納されたロボットアーム100の形状データを基にして対象となる当該中間点におけるロボットアーム100の形状(姿勢)を求めて、作業空間記憶手段44に格納された障害物Ob(干渉領域CA)の形状データと干渉するかどうかをチェックする。
As shown in FIGS. 3 and 4A, the intermediate point interference check unit 34 is an arithmetic unit that determines whether the intermediate point obtained by the intermediate point calculation unit 33 interferes with the obstacle Ob.
That is, the intermediate point interference check unit 34 is stored in the robot shape storage unit 43 from the joint angles (θ1, θ2, θ3, θ4, θ5, θ6) of the robot arm 100 (see FIG. 1) at the target intermediate point. Based on the shape data of the robot arm 100, the shape (posture) of the robot arm 100 at the target intermediate point is obtained, and the shape data of the obstacle Ob (interference area CA) stored in the work space storage means 44 Check if it interferes with.

例えば、中間点干渉チェック手段34は、ロボットアーム100と障害物ObのCADデータをポリゴン(例えば三角形等の多角形)の集合として表した場合には、対象となる現在位置隣接コンフィギュレーション点におけるロボットアーム100のポリゴンの集合と障害物Obのポリゴンの集合が幾何学的に接触しているかどうかを判定して判断する。   For example, when the CAD data of the robot arm 100 and the obstacle Ob is represented as a set of polygons (for example, polygons such as triangles), the intermediate point interference checking unit 34 is the robot at the configuration point adjacent to the target current position. It is determined by determining whether the polygon set of the arm 100 and the polygon set of the obstacle Ob are in geometric contact.

具体的には、中間点干渉チェック手段34は、Cスペース上において、対象となる中間点qm(qm1,qm2,・・・)におけるロボットアーム100の形状を構成する1つの多角形(ポリゴン)と障害物Obの形状を構成する1つの多角形(ポリゴン)との接触を判定し、それぞれすべての多角形(ポリゴン)の集合について同様に判定して、ロボットアーム100と障害物Obが干渉するか否かを判断して、ディスプレイ21(図3)に表示させることができる。   Specifically, the intermediate point interference check unit 34 includes one polygon (polygon) constituting the shape of the robot arm 100 at the target intermediate point qm (qm1, qm2,...) On the C space. Whether contact with one polygon (polygon) constituting the shape of the obstacle Ob is determined, and a determination is similarly made for each set of all polygons (polygons), so that the robot arm 100 and the obstacle Ob interfere with each other. It can be determined and displayed on the display 21 (FIG. 3).

なお、本実施形態においては、Cスペース上において、ポリゴンの集合が接触しているかどうかで判断したが、これに限定されるものではなく、ロボットアーム100の自由度数(N次元)に応じて、このN次元形状の干渉領域CAを想定してロボットアーム100と干渉領域CAとの干渉をチェックしてもよい。
また、ポリゴン(多角形状)ではなく、ロボットアーム100と障害物Obを点や線ないし面の集合として表して、このような点と面との干渉チェック、または線と面との干渉チェックでもよい。
さらに、Cスペース上における干渉チェックに限定されるものではなく、CAD空間や実空間内で点や線ないしポリゴンを利用した干渉チェックを採用することもできる。
In this embodiment, it is determined whether or not a set of polygons is in contact with each other on the C space. However, the present invention is not limited to this, and according to the degree of freedom (N-dimensional) of the robot arm 100, The interference between the robot arm 100 and the interference area CA may be checked assuming this N-dimensional shape of the interference area CA.
Further, instead of polygons (polygonal shapes), the robot arm 100 and the obstacle Ob may be represented as a set of points, lines, or surfaces, and such a point-to-surface interference check or a line-to-surface interference check may be used. .
Furthermore, the present invention is not limited to the interference check on the C space, and an interference check using points, lines, or polygons in the CAD space or the real space can also be adopted.

続いて、本発明の実施形態に係る円滑経路生成装置1における動作について図4Aないし図4B、および適宜図5のフローチャートを参照しながら説明する。
[データの格納]
作業者(不図示)が、初期コンフィギュレーション点qiから目標コンフィギュレーション点qgまで障害物Obを回避するように予め生成されたロボットアーム100(図1)の動作経路(図4A(a)を参照)、初期位置データ、目標位置データ、ロボットアーム100の形状データ、および障害物Ob(図2(a)、図8)の形状データを操作盤2から入力すると、円滑経路生成装置1は、図5に示すように、Cスペース上において、ロボットアーム100の初期位置データを初期コンフィギュレーション点qiとして初期位置記憶手段41に格納し、目標位置データを目標コンフィギュレーション点qgとして目標位置記憶手段42に格納し、動作経路(図4A(a))を動作経路記憶手段45に格納し、ロボットアーム100(図1参照)の形状データをロボット形状記憶手段43に格納し、障害物Ob(図2(a)、図8)の形状データを作業空間記憶手段44に格納する(図5のステップS1参照)。
Next, the operation of the smooth path generation device 1 according to the embodiment of the present invention will be described with reference to FIGS. 4A to 4B and the flowchart of FIG. 5 as appropriate.
[Store data]
An operator (not shown) refers to an operation path (see FIG. 4A (a)) of the robot arm 100 (FIG. 1) generated in advance so as to avoid the obstacle Ob from the initial configuration point qi to the target configuration point qg. ), The initial position data, the target position data, the shape data of the robot arm 100, and the shape data of the obstacle Ob (FIGS. 2A and 8) are input from the operation panel 2, the smooth path generation device 1 As shown in FIG. 5, in the C space, the initial position data of the robot arm 100 is stored in the initial position storage means 41 as the initial configuration point qi, and the target position data is stored in the target position storage means 42 as the target configuration point qg. The motion path (FIG. 4A (a)) is stored in the motion path storage means 45, and the robot arm 100 (FIG. Storing the shape data of the reference) to the robot shape storage section 43, the obstacle Ob (FIG. 2 (a), stores the shape data of FIG. 8) in the work space memory means 44 (see step S1 in FIG. 5).

[経路途中点の設定]
円滑経路生成装置1は、図4A(a)に示すように、経路途中点設定手段31(図3)により、複数の経路途中点として、初期コンフィギュレーション点qi(c,1)、目標コンフィギュレーション点qg(i,9)、および動作経路における折曲点であるコンフィギュレーション点(e,3),(e,4),(h,4),(h,7),(i,8)を設定する(図5のステップS2)。
[Set route halfway point]
As shown in FIG. 4A (a), the smooth path generation device 1 uses the path halfway point setting means 31 (FIG. 3) as an initial configuration point qi (c, 1), target configuration as a plurality of path halfway points. A point qg (i, 9) and configuration points (e, 3), (e, 4), (h, 4), (h, 7), (i, 8) which are bending points in the operation path are represented. Setting is made (step S2 in FIG. 5).

[経路途中点対の採択]
円滑経路生成装置1は、経路途中点対採択手段32(図3)により、図4A(b)に示すように、対となる経路途中点対q1(c,1),q2(e,4)を採択し、図4A(c)に示すように、対となる経路途中点対q2(e,4),q3(h,7)を採択する。また、図4B(a)に示すように、対となる経路途中点対q3(e,4),q4(h,7)を採択する(図5のステップS3参照)。
[Adoption of point pairs along the route]
As shown in FIG. 4A (b), the smooth route generation device 1 uses a route halfway point pair selection means 32 (FIG. 3) to form a pair of route halfway points q1 (c, 1), q2 (e, 4). As shown in FIG. 4A (c), a pair of route intermediate points q2 (e, 4) and q3 (h, 7) to be paired is adopted. Further, as shown in FIG. 4B (a), a pair of route halfway points q3 (e, 4) and q4 (h, 7) to be paired are adopted (see step S3 in FIG. 5).

[中間点の演算]
円滑経路生成装置1は、図4A(b)に示すように、中間点演算手段33(図3)により、一の経路途中点であるq1(c,1)と他の経路途中点であるq2(e,4)との中間点qm1(d,2−3)を、計算式(q1の位置データ+q2の位置データ)/2から求める(図5のステップS4参照)。ここで、中間点の位置を図上で特定するために説明の便宜上、横のグリッド線2とグリッド線3の間の位置を「2−3」のように表示する。
[Calculation of midpoint]
As shown in FIG. 4A (b), the smooth route generation device 1 uses the intermediate point calculation means 33 (FIG. 3) to q1 (c, 1) which is one route intermediate point and q2 which is another route intermediate point. An intermediate point qm1 (d, 2-3) with (e, 4) is obtained from the calculation formula (position data of q1 + position data of q2) / 2 (see step S4 in FIG. 5). Here, for the sake of convenience of description in order to specify the position of the intermediate point on the drawing, the position between the horizontal grid line 2 and the grid line 3 is displayed as “2-3”.

円滑経路生成装置1は、図4A(c)に示すように、中間点演算手段33により、一の経路途中点であるq2(e,4)と他の経路途中点であるq3(h,7)との中間点qm2(f−g,5−6)を、計算式(q2の位置データ+q3の位置データ)/2から求める。ここで、中間点の位置を図上で特定するために説明の便宜上、横のグリッド線fとグリッド線gの間の位置を「f−g」のように表示する。   As shown in FIG. 4A (c), the smooth route generation device 1 uses the intermediate point calculation means 33 to make q2 (e, 4) which is one route intermediate point and q3 (h, 7) which is another route intermediate point. ) And the intermediate point qm2 (f−g, 5-6) is obtained from the calculation formula (position data of q2 + position data of q3) / 2. Here, for the convenience of explanation in order to specify the position of the intermediate point on the drawing, the position between the horizontal grid line f and the grid line g is displayed as “f−g”.

同様に、円滑経路生成装置1は、図4B(a)に示すように、中間点演算手段33により、一の経路途中点であるq3(h,7)と他の経路途中点であるq4(i,9)との中間点qm3(h−i,8−9)を、計算式(q3の位置データ+q4の位置データ)/2から求める。   Similarly, as shown in FIG. 4B (a), the smooth route generation device 1 uses the intermediate point calculation means 33 to generate q3 (h, 7) as one route intermediate point and q4 ( The intermediate point qm3 (hi, 8-9) with i, 9) is obtained from the calculation formula (position data of q3 + position data of q4) / 2.

[中間点の干渉チェック]
円滑経路生成装置1は、図4A(b)と(c)、および図4B(a)に示すように、中間点干渉チェック手段34(図3)により、中間点演算手段33により求めた中間点qm1(d,2−3)、中間点qm2(f−g,5−6)、および中間点qm3(h−i,8−9)が障害物Ob(干渉領域CA)と干渉するかどうかを判断する(図5のステップS5参照)。
[Interference check at midpoint]
As shown in FIGS. 4A (b) and 4 (c) and FIG. 4B (a), the smooth path generation device 1 uses the intermediate point calculation means 33 by the intermediate point interference check means 34 (FIG. 3). Whether qm1 (d, 2-3), intermediate point qm2 (f-g, 5-6), and intermediate point qm3 (hi, 8-9) interfere with obstacle Ob (interference area CA). Judgment is made (see step S5 in FIG. 5).

すなわち、図4A(b)に示す中間点qm1(d,2−3)は、障害物Obとの干渉点(d,5)から離れているので、中間点干渉チェック手段34は、中間点qm1(d,2−3)が干渉領域CAに含まれておらず、障害物Obと干渉しないと判断することができる。
一方、図4(c)に示す中間点qm2(f−g,5−6)は、干渉領域CAに含まれているので、中間点干渉チェック手段34は、中間点qm2(f−g,5−6)が障害物Obと干渉すると判断する。
そして、図4B(a)に示す中間点qm3(h−i,8−9)についても、中間点干渉チェック手段は、中間点qm1(d,2−3)の場合と同様にして、中間点qm3(h−i,8−9)が障害物Obと干渉しないと判断する。
That is, since the intermediate point qm1 (d, 2-3) shown in FIG. 4A (b) is away from the interference point (d, 5) with the obstacle Ob, the intermediate point interference check means 34 is connected to the intermediate point qm1. It can be determined that (d, 2-3) is not included in the interference area CA and does not interfere with the obstacle Ob.
On the other hand, since the intermediate point qm2 (f−g, 5-6) shown in FIG. 4C is included in the interference area CA, the intermediate point interference check unit 34 determines the intermediate point qm2 (f−g, 5 It is determined that −6) interferes with the obstacle Ob.
Then, for the intermediate point qm3 (hi, 8-9) shown in FIG. 4B (a), the intermediate point interference checking means performs the intermediate point in the same manner as the intermediate point qm1 (d, 2-3). It is determined that qm3 (hi, 8-9) does not interfere with the obstacle Ob.

このようにして、中間点qm(qm1,qm2,qm3)が障害物Ob(干渉領域CA)と干渉するかどうかを判断した上で、円滑経路生成装置1は、中間点干渉チェック手段34により、中間点qm(qm2)が障害物Obと干渉すると判断された場合には、中間点qm(qm2)を新経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し(図4A(c)、図5のS7参照)、中間点qm(qm1,qm3)が干渉しないと判断された場合には、中間点qm(qm1,qm3)を新経路途中点として採択し、動作経路を経路途中点対を構成する一の経路途中点(q1,q3)から新経路途中点(qm1,qm3)を通って前記他の経路途中点(q2,q4)まで進む新動作経路に変更して、円滑経路を再生成する円滑経路生成ステップを実行する(図4A(b)、図5のS6参照)。   Thus, after determining whether or not the intermediate point qm (qm1, qm2, qm3) interferes with the obstacle Ob (interference area CA), the smooth path generation device 1 uses the intermediate point interference check unit 34 to If it is determined that the intermediate point qm (qm2) interferes with the obstacle Ob, the current route maintaining step of adopting the current operation route without adopting the intermediate point qm (qm2) as the midpoint of the new route is performed. If it is determined that the intermediate point qm (qm1, qm3) does not interfere with the intermediate point qm (qm1, qm3), the intermediate point qm (qm1, qm3) is selected as the new route halfway point. Then, the new operation of moving the operation route from one route intermediate point (q1, q3) constituting the route intermediate point pair to the other route intermediate point (q2, q4) through the new route intermediate point (qm1, qm3). Change to route, smooth route Run smoothly path generating step for regenerating (FIG 4A (b), reference S6 in FIG. 5).

[円滑経路生成ステップ]
具体的には、図4A(b)では、中間点qm1(d,2−3)は障害物Ob(干渉領域CA)と干渉しないので、円滑経路生成装置1は、円滑経路生成ステップを実行する。
つまり、円滑経路生成ステップでは、中間点qm1(d,2−3)を新経路途中点として採択し、コンフィギュレーション点q1(c,1)からコンフィギュレーション点qt1(e,3)を通ってコンフィギュレーション点q2(e,4)まで進む現在の動作経路(太実線で表示)をコンフィギュレーション点q1(c,1)から新経路途中点である中間点qm1(d,2−3)を通ってコンフィギュレーション点q2(e,4)まで進む新動作経路(破線で表示)に変更して円滑経路を再生成する(図5のS6参照)。
[Smooth path generation step]
Specifically, in FIG. 4A (b), since the intermediate point qm1 (d, 2-3) does not interfere with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the smooth path generation step. .
That is, in the smooth route generation step, the intermediate point qm1 (d, 2-3) is adopted as a new route halfway point, and the configuration point q1 (c, 1) is passed through the configuration point qt1 (e, 3). The current operation path (indicated by a thick solid line) that travels to the point q2 (e, 4) passes from the configuration point q1 (c, 1) through the intermediate point qm1 (d, 2-3) that is the midpoint of the new path. The smooth route is regenerated by changing to a new operation route (indicated by a broken line) that goes to the configuration point q2 (e, 4) (see S6 in FIG. 5).

[現在経路維持ステップ]
一方、図4A(c)では、中間点qm2(f−g,5−6)は、障害物Ob(干渉領域CA)と干渉するので、円滑経路生成装置1は、現在経路維持ステップを実行する。
具体的には、中間点qm2(f−g,5−6)を新経路途中点として採択せずに、コンフィギュレーション点q2(e,4)から経路途中点qt2(h,4)を通ってコンフィギュレーション点q3(h,7)まで進む現在のままの動作経路を採択して維持する。
[Current route maintenance step]
On the other hand, in FIG. 4A (c), since the intermediate point qm2 (f-g, 5-6) interferes with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the current path maintaining step. .
Specifically, the intermediate point qm2 (f-g, 5-6) is not adopted as the new route halfway point, and passes from the configuration point q2 (e, 4) to the route halfway point qt2 (h, 4). Adopt and maintain the current operating path that goes to configuration point q3 (h, 7).

図4B(a)では、中間点qm3(h−i,8)は障害物Ob(干渉領域CA)と干渉しないので、円滑経路生成装置1は、円滑経路生成ステップを実行し、コンフィギュレーション点q3(h,7)から新経路途中点qm3(h−i,8)を通って目標コンフィギュレーション点qgであるコンフィギュレーション点q4(i,9)まで進む新動作経路に変更して円滑経路を再生成する。   In FIG. 4B (a), since the intermediate point qm3 (hi, 8) does not interfere with the obstacle Ob (interference area CA), the smooth path generation device 1 executes the smooth path generation step, and the configuration point q3 A smooth path is reproduced by changing to a new operation path from (h, 7) to the configuration point q4 (i, 9), which is the target configuration point qg, through the new path halfway point qm3 (hi, 8). To do.

このようにして、円滑経路生成装置1は、図4B(b)に示すように、予め生成されたロボットアーム100の動作経路(図4A(a)参照)を円滑化して、初期コンフィギュレーション点qi(c,1)から、途中点q2(e,4),qt2(h,4),q3(h,7)を通って,目標コンフィギュレーション点qg(i,9)まで進む円滑経路を再生成する。   In this way, as shown in FIG. 4B (b), the smooth path generation device 1 smoothes the motion path (see FIG. 4A (a)) of the robot arm 100 that has been generated in advance, and the initial configuration point qi. A smooth path that travels from (c, 1) to the target configuration point qg (i, 9) through intermediate points q2 (e, 4), qt2 (h, 4), q3 (h, 7) is regenerated. To do.

[シフト経路途中点対採択ステップ]
続いて、シフト経路途中点対採択ステップについて、図3、図4B(c)、および図5を参照しながら説明する。シフト経路途中点対採択ステップは、円滑経路生成ステップの後、経路途中点対採択手段32により、一の経路途中点および他の経路途中点のうち少なくとも一方が異なる一のシフト経路途中点、および他のシフト経路途中点からなる一対のシフト経路途中点対を採択する(図5のS8)。
[Shift path halfway point pair selection step]
Next, the shift path midpoint pair selection step will be described with reference to FIGS. 3, 4B (c), and 5. FIG. The shift route halfway point adopting step includes, after the smooth route generating step, one route halfway point in which at least one of one route halfway point and another route halfway point is different by the route halfway point pair adopting unit 32, and A pair of shift path midpoint pairs consisting of other shift path midpoints is adopted (S8 in FIG. 5).

すなわち、円滑経路生成装置1は、円滑経路生成ステップにおける図4A(b)の動作経路では、経路途中点対採択手段32(図3参照)により、対となる経路途中点対q1(c,1),q2(e,4)を採択したが、シフト経路途中点対採択ステップにおける図4B(b)の動作経路では、q2(e,4)からずらした位置にあるqt2(h,4)を採択して、シフト経路途中点対として、q1(c,1),qt2(h,4)を採択する。   That is, in the smooth route generation step, the smooth route generation device 1 uses the route intermediate point pair selection means 32 (see FIG. 3) to form a pair of route intermediate point pairs q1 (c, 1) in the operation route of FIG. 4A (b). ), Q2 (e, 4) are adopted, but qt2 (h, 4) at a position shifted from q2 (e, 4) in the operation route of FIG. Then, q1 (c, 1) and qt2 (h, 4) are adopted as the shift path halfway point pair.

同様にして、円滑経路生成装置1は、円滑経路生成ステップにおける図4B(a)の動作経路では、経路途中点対採択手段32(図3参照)により、対となる経路途中点対q3(h,7),q4(i,9)を採択したが、シフト経路途中点対採択ステップにおける図4B(b)の動作経路では、q3(h,7)からずらした位置にあるqt2(h,4)を採択して、シフト経路途中点対として、qt2(h,4),q4(i,9)を採択する。   Similarly, in the smooth route generation step, the smooth route generation device 1 uses the route halfway point selection means 32 (see FIG. 3) to form a pair of route halfway points q3 (h) in the operation route of FIG. 4B (a). 7), q4 (i, 9) are adopted, but qt2 (h, 4) at a position shifted from q3 (h, 7) in the operation route of FIG. ) Is adopted, and qt2 (h, 4) and q4 (i, 9) are adopted as shift path midpoint pairs.

なお、本実施形態においては、シフト経路途中点対採択ステップにおいて、複数のシフト途中点対を連続して採択したが、これに限定されるものではなく、まず1つのシフト途中点対を採択し、後記するシフト中間点の干渉チェックステップを実行し、繰り返し他のシフト途中点対を採択するというようにして、円滑経路生成ステップの後にシフト途中点対採択ステップとシフト中間点の干渉チェックステップを順次繰り返して実行しながらシフト円滑経路を生成することもできる。   In this embodiment, in the shift path midpoint pair selection step, a plurality of shift midpoint pairs are continuously selected. However, the present invention is not limited to this. First, one shift midpoint pair is selected. Then, the shift intermediate point interference check step, which will be described later, is executed, and other shift midpoint pairs are repeatedly adopted, so that the shift midpoint pair selection step and the shift intermediate point interference check step are performed after the smooth path generation step. It is also possible to generate a smooth shift path while repeatedly performing it sequentially.

例えば、q1(c,1),qt1(e,3),q2(e,4)の3箇所を経路途中点として採択して、円滑経路生成ステップによりqt1(e,3)からqm1(d,2−3)に変更して円滑にした後に(図4A(b)参照)、qm1(d,2−3),q2(e,4),qt2(h,4)の3箇所の経路途中点のうちから、シフト経路途中点対採択ステップによりqm1(d,2−3)とqt2(h,4)をシフト経路途中点対として採択して(図4B(b)参照)、後記するシフト中間点の干渉チェックステップにより中間点の干渉をチェックしてシフト円滑経路を生成するというように、初期コンフィギュレーション点qi(c,1)から順次目標コンフィギュレーション点qg(i,9)までシフト経路途中点対をずらしながら採択して、シフト途中点対採択ステップとシフト中間点の干渉チェックステップを順次繰り返して実行することもできる。   For example, three points q1 (c, 1), qt1 (e, 3), and q2 (e, 4) are adopted as route intermediate points, and qt1 (e, 3) to qm1 (d, After changing to 2-3) and smoothing (see FIG. 4A (b)), three route halfway points of qm1 (d, 2-3), q2 (e, 4), and qt2 (h, 4) Among them, qm1 (d, 2-3) and qt2 (h, 4) are adopted as shift path halfway point pairs in the shift path halfway point adoption step (see FIG. 4B (b)), and the shift middle described later is performed. In the middle of the shift path from the initial configuration point qi (c, 1) to the target configuration point qg (i, 9), such as generating a smooth shift path by checking for interference at the intermediate point in the point interference check step. Adopt while shifting the point pair It can be successively repeatedly perform the interference checking step of shifting the middle point pairs adoption steps and shift midpoint.

[シフト中間点の干渉チェックステップ]
続いて、円滑経路生成装置1は、中間点演算手段33(図3参照)により、一の経路途中点であるq1(c,1)と他の経路途中点であるqt2(h,4)との中間点であるシフト中間点qm4(e−f,2−3)を、計算式(q1の位置データ+qt2の位置データ)/2から求めて、図4B(b)に示すように、中間点干渉チェック手段34(図3参照)により、中間点演算手段33により求めたシフト中間点qm4(e−f,2−3)が障害物Obと干渉するかどうかを判断する(図5のS9)。
[Interference check step at shift midpoint]
Subsequently, the smooth route generation device 1 uses the intermediate point calculation means 33 (see FIG. 3) to determine q1 (c, 1) as one route intermediate point and qt2 (h, 4) as another route intermediate point. The shift intermediate point qm4 (ef, 2-3), which is the intermediate point of, is obtained from the calculation formula (position data of q1 + position data of qt2) / 2, and as shown in FIG. 4B (b) It is determined by the interference check means 34 (see FIG. 3) whether or not the shift intermediate point qm4 (ef, 2-3) obtained by the intermediate point calculation means 33 interferes with the obstacle Ob (S9 in FIG. 5). .

同様にして、円滑経路生成装置1は、中間点演算手段33(図3参照)により、一の経路途中点であるqt2(h,4)と他の経路途中点であるq4(i,9)との中間点であるシフト中間点qm5(h−i,6−7)を、計算式(qt2の位置データ+q4の位置データ)/2から求めて、図4B(b)に示すように、中間点干渉チェック手段34(図3参照)により、中間点演算手段33により求めたシフト中間点qm5(h−i,6−7)が障害物Obと干渉するかどうかを判断する(図5のS9)。   Similarly, the smooth route generation apparatus 1 uses the intermediate point calculation means 33 (see FIG. 3) to qt2 (h, 4) that is one route intermediate point and q4 (i, 9) that is another route intermediate point. The shift intermediate point qm5 (hi, 6-7), which is the intermediate point between the two, is obtained from the calculation formula (position data of qt2 + position data of q4) / 2, and as shown in FIG. 4B (b) The point interference check means 34 (see FIG. 3) determines whether or not the shift intermediate point qm5 (hi, 6-7) obtained by the intermediate point calculation means 33 interferes with the obstacle Ob (S9 in FIG. 5). ).

このようにして、中間点qm4,qm5が障害物Ob(干渉領域CA)と干渉するかどうかを判断した上で、シフト中間点qm4,qm5が障害物Obと干渉すると判断された場合には、シフト中間点qm4,qm5を新たな経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し(図5のS11)、障害物Obと干渉しないと判断された場合には、中間点qm4,qm5を新経路途中点として採択し、動作経路を一のシフト経路途中点(q1,qt2)から新経路途中点(qm4,qm5)を通って前記他のシフト経路途中点(qt2,q4)まで進む新動作経路に変更して前記円滑経路を再生成するシフト円滑経路生成ステップを実行する(図5のS10)。   In this way, after determining whether or not the intermediate points qm4 and qm5 interfere with the obstacle Ob (interference area CA), if it is determined that the shifted intermediate points qm4 and qm5 interfere with the obstacle Ob, A current route maintenance step is adopted in which the current operation route is adopted without adopting the shift intermediate points qm4 and qm5 as new route intermediate points (S11 in FIG. 5), and it is determined that there is no interference with the obstacle Ob. In this case, the intermediate points qm4 and qm5 are adopted as the new route intermediate points, and the operation route is changed from one shift route intermediate point (q1, qt2) through the new route intermediate point (qm4, qm5) to the other shift route. A shift smooth path generation step is performed in which the smooth path is regenerated by changing to a new motion path that goes to the midpoint (qt2, q4) (S10 in FIG. 5).

ここで、シフト中間点qm4,qm5は、図4B(b)に示すように、それぞれ障害物Ob(干渉領域CA)と干渉しないため、円滑経路生成装置1は、コンフィギュレーション点q1(c,1)からコンフィギュレーション点q2(e,4)を通ってコンフィギュレーション点qt2(e,4)まで進み、コンフィギュレーション点q3(h,7)を通ってコンフィギュレーション点q4(i,9)まで進む現在の動作経路(太実線で表示)をコンフィギュレーション点q1(c,1)から新経路途中点である中間点qm4(e−f,2−3)を通り、qt2(h,4)から新経路途中点である中間点qm5(h−i,6−7)を通って目標コンフィギュレーション点q4(i,9)まで進む新動作経路(破線で表示)に変更してシフト円滑経路を再生成する(図4B(c)、図5のS10参照)。   Here, since the shift intermediate points qm4 and qm5 do not interfere with the obstacle Ob (interference area CA) as shown in FIG. 4B (b), the smooth path generation device 1 is configured with the configuration points q1 (c, 1 ) To configuration point qt2 (e, 4) through configuration point q2 (e, 4) and to configuration point q4 (i, 9) through configuration point q3 (h, 7) From the configuration point q1 (c, 1) through the intermediate point qm4 (ef, 2-3), which is an intermediate point of the new route, and the new route from qt2 (h, 4). Change to a new motion path (indicated by a broken line) that goes to the target configuration point q4 (i, 9) through the intermediate point qm5 (hi, 6-7), which is an intermediate point, and a shift circle Regenerating pathway (FIG. 4B (c), reference S10 in FIG. 5).

このようにして、円滑経路生成装置1は、円滑経路生成ステップの後に現在経路維持ステップまたはシフト円滑経路生成ステップを実行し、円滑経路生成ステップから現在経路維持ステップまたはシフト円滑経路生成ステップを順次繰り返して動作経路を円滑化することで、円滑経路生成ステップにおける円滑処理からさらに円滑な動作経路を順次生成することができる。   In this way, the smooth route generation apparatus 1 executes the current route maintenance step or the shift smooth route generation step after the smooth route generation step, and sequentially repeats the current route maintenance step or the shift smooth route generation step from the smooth route generation step. By smoothing the operation path, smoother operation paths can be sequentially generated from the smooth process in the smooth path generation step.

このため、円滑経路生成装置1は、初期位置qiから目標位置qgまでの動作経路において、迂回した経路や屈曲した経路のような振動や余分な動作を誘発する不適切な経路が含まれている場合には、迂回した経路や屈曲した経路から逐次直線的な経路に変更し、経路の長さをしだいに短縮して動作経路を円滑にして、ロボットアーム100の動作における加速度の急激な変化を抑制することができる。   For this reason, the smooth path generation device 1 includes an inappropriate path that induces vibrations and extra movements such as a detour path and a bent path in the operation path from the initial position qi to the target position qg. In this case, the path is changed from a detoured path or a bent path to a linear path successively, the path length is gradually shortened to make the motion path smooth, and a sudden change in acceleration in the motion of the robot arm 100 is caused. Can be suppressed.

以上、本発明の実施形態について説明したが、本発明は前記した実施形態に限定されず、適宜変更して実施することが可能である。
例えば、本実施形態においては、経路途中点設定手段31により、Cスペース上における折曲点を選定したが、これに限定されるものではなく、直線経路を複数に分割した分割点、動作経路を任意に等分した点等、動作経路の形態や用途等に応じて適宜設定することができる。
Although the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and can be implemented with appropriate modifications.
For example, in the present embodiment, the bending point on the C space is selected by the route halfway point setting means 31, but the present invention is not limited to this. Arbitrarily divided points and the like can be set as appropriate according to the form and use of the operation path.

具体的に以下、本発明における経路途中点設定手段31による第1の実施例について、図6を参照しながら説明する。図6は、本発明の実施形態に係る図4B(c)の動作経路を円滑にする他の実施例を説明するための図であり、(a)は折曲点qt2(h,4)の円滑処理、(b)は分割点qm4(e−f,2−3)と分割点qm5(h−i,6−7)の円滑処理、(c)は生成された円滑経路を示す。   Specifically, the first embodiment of the route halfway point setting means 31 according to the present invention will be described below with reference to FIG. FIG. 6 is a diagram for explaining another example of smoothing the operation path of FIG. 4B (c) according to the embodiment of the present invention, and FIG. 6 (a) is a diagram of the bending point qt2 (h, 4). Smooth processing, (b) shows smooth processing of the dividing point qm4 (ef, 2-3) and dividing point qm5 (hi, 6-7), and (c) shows the generated smooth path.

本発明における第1の実施例に係る経路途中点設定手段31(図3参照)は、図6(a)に示すように、経路途中点として、qi(e,1)、qg(i,9)、折曲点qt2(h,4)の他、qi(e,1)からqt2(h,4)に至る直線経路を2分する分割点qm4(e−f,2−3)と,qt2(h,4)からqg(i,9)に至る直線経路を2分する分割点qm5(h−i,6−7)と、を経路途中点として設定したものである。   As shown in FIG. 6 (a), the route halfway point setting means 31 (see FIG. 3) according to the first embodiment of the present invention uses qi (e, 1), qg (i, 9) as route halfway points. ), The bending point qt2 (h, 4), the dividing point qm4 (ef, 2-3) that bisects the straight path from qi (e, 1) to qt2 (h, 4), and qt2. A dividing point qm5 (hi, 6-7) that bisects a straight line route from (h, 4) to qg (i, 9) is set as a route halfway point.

そして、経路途中点対採択手段32により、折曲点であるqt2(h,4)を対象として、この折曲点qt2(h,4)を挟むようにして分割点qm4(e−f,2−3)と分割点qm5(h−i,6−7)を採択し、中間点演算手段33により中間点qm6(g,4−5)を求め、障害物Obとの干渉をチェックした上で新経路途中点として採択することができる。
このようにして、再生成された円滑経路は、図6(b)の太実線で示すように、qi(e,1)、から分割点qm4(e−f,2−3)、中間点qm6(g,4−5)、分割点qm5(h−i,6−7)を通ってqg(i,9)まで進む円滑な経路を構成する。
Then, the route halfway point pair selection means 32 targets the bending point qt2 (h, 4) and divides the bending point qt2 (h, 4) so as to sandwich the dividing point qm4 (ef, 2-3). ) And the division point qm5 (hi, 6-7), the intermediate point calculation means 33 obtains the intermediate point qm6 (g, 4-5), and checks the interference with the obstacle Ob before the new route. Can be adopted as an intermediate point.
Thus, as shown by the thick solid line in FIG. 6B, the regenerated smooth path is divided from qi (e, 1) to the dividing point qm4 (ef, 2-3) and the intermediate point qm6. (G, 4-5), a smooth path that passes through the dividing point qm5 (hi, 6-7) to qg (i, 9) is formed.

ここで、さらに、図6(b)に示すように、経路途中点対採択手段32により、経路途中点対が数珠つながりに連続するように、折曲点を3箇所ずつ連続して選定し、選定した3箇所の折曲点において、両側の折曲点のうちの一方を一の経路途中点とし、他方を他の経路途中点とすることで、図4B(a)の場合と同様にして、qi(e,1)から中間点qm7(e,3)、中間点qm6(g,4−5)、中間点qm8(h,7)を通ってqg(i,9)まで進むさらに円滑な経路を生成することができる(図6(b)の破線、図6(c)参照)。   Here, as shown in FIG. 6 (b), the path halfway point pair selection means 32 continuously selects the bending points three by three so that the path middle point pairs are connected in a daisy chain, In the selected three bending points, one of the bending points on both sides is set as one route halfway point and the other is set as the other route halfway point, similarly to the case of FIG. 4B (a). , Qi (e, 1) to intermediate point qm7 (e, 3), intermediate point qm6 (g, 4-5), intermediate point qm8 (h, 7) to qg (i, 9) A route can be generated (see broken line in FIG. 6B, FIG. 6C).

なお、第1の実施例における分割点qm4(e−f,2−3)と分割点qm5(h−i,6−7)は、それぞれ前記した実施形態におけるqi(e,1)とqt2(h,4)、およびqt2(h,4)とq4(I,9)の中間点でもあるので(図4B(b)参照)、新経路途中点として採択された中間点を経路途中点設定手段31(図3参照)により追加して設定された経路途中点として、経路途中点記憶手段46に格納して利用するようにしてもよい。   The dividing point qm4 (ef, 2-3) and the dividing point qm5 (hi, 6-7) in the first example are respectively the qi (e, 1) and qt2 ( h, 4) and qt2 (h, 4) and q4 (I, 9) are also intermediate points (see FIG. 4B (b)), and the intermediate point selected as the new route intermediate point is the route intermediate point setting means. The route halfway point additionally set by 31 (see FIG. 3) may be stored in the route halfway point storage means 46 and used.

続いて、本発明における経路途中点設定手段31による第2の実施例について、図7を参照しながら説明する。第2の実施例に係る経路途中点設定手段31(図3参照)は、図7(a)に示すように、円滑処理する折曲点としてqt2に着目し、q5(f−g,4)、q6(h,5−6)と、を経路途中点対として設定したものである。
すなわち、経路途中点設定手段31は、初期コンフィギュレーション点qi(c,1)と目標コンフィギュレーション点qgを結ぶ線Lから最も遠い距離δに位置する折曲点qt2(h,4)を経路途中点として設定し、この折曲点qt2(h,4)を挟むようにして折曲点q2(e,4)と折曲点qt2(h,4)の分割点q5(f−g,4)、および折曲点qt2(h,4)と折曲点q3(h,7)の分割点q6(h,5−6)をさらに経路途中点として設定したものである。
Next, a second embodiment of the route halfway point setting unit 31 according to the present invention will be described with reference to FIG. As shown in FIG. 7A, the route halfway point setting unit 31 (see FIG. 3) according to the second embodiment pays attention to qt2 as a bending point to be smoothly processed, and q5 (f−g, 4). , Q6 (h, 5-6) are set as a route halfway point pair.
That is, the route halfway point setting means 31 passes the bending point qt2 (h, 4) located at the distance δ farthest from the line L connecting the initial configuration point qi (c, 1) and the target configuration point qg. And a dividing point q5 (f−g, 4) between the bending point q2 (e, 4) and the bending point qt2 (h, 4) so as to sandwich the bending point qt2 (h, 4), and A dividing point q6 (h, 5-6) between the bending point qt2 (h, 4) and the bending point q3 (h, 7) is further set as a midway point.

そして、経路途中点対採択手段32により、これらの3箇所の経路途中点qt2(h,4)、q5(f−g,4)、q6(h,5−6)のうち、q5(f−g,4)、およびq6(h,5−6)を経路途中点対q5,q6としてを採択して、中間点演算手段33により中間点qm9(g−h,4−5)を求め、障害物Obとの干渉をチェックした上で新経路途中点として採択する。   Then, the route halfway point selection means 32 causes q5 (f−) of these three route halfway points qt2 (h, 4), q5 (f−g, 4), and q6 (h, 5-6). g, 4) and q6 (h, 5-6) are adopted as the route halfway point pairs q5 and q6, and the intermediate point calculation means 33 obtains the intermediate point qm9 (g−h, 4-5). After checking the interference with the object Ob, it is adopted as a new route halfway point.

このようにして、再生成された円滑経路は、図7(b)の太実線で示すように、qi(e,1)、から分割点q5(f−g,4)を通り、分割点q6(h,5−6)まで進むため、初期コンフィギュレーション点qi(c,1)と目標コンフィギュレーション点qgを結ぶ線Lから最も遠い折曲点qt2(h,4)を排除することで円滑な経路を生成することができる。
さらに図7(b)において、線Lから最も遠い折曲点q6(h,5−6)に続いて、折曲点q5(f−g,4)についても同様に円滑処理をすることで、漸次円滑な経路を生成することができる。
In this way, the regenerated smooth path passes from qi (e, 1) to the dividing point q5 (f-g, 4) as shown by the thick solid line in FIG. Since the process proceeds to (h, 5-6), the bending point qt2 (h, 4) farthest from the line L connecting the initial configuration point qi (c, 1) and the target configuration point qg is eliminated. A route can be generated.
Further, in FIG. 7B, following the bending point q6 (h, 5-6) farthest from the line L, smooth processing is similarly performed for the bending point q5 (f-g, 4), A gradually smooth path can be generated.

1 円滑経路生成装置
2 操作盤
3 演算処理装置
4 記憶装置
31 経路途中点設定手段
32 経路途中点対採択手段
33 中間点演算手段
34 中間点干渉チェック手段
41 初期位置記憶手段
42 目標位置記憶手段
43 ロボット形状記憶手段
44 作業空間記憶手段
45 動作経路記憶手段
46 経路途中点記憶手段
100,100′ ロボットアーム(多軸ロボットアーム)
CA 干渉領域
Ob 障害物
q1,q2 経路途中点対
q2,q3 経路途中点対
q3,q4 経路途中点対
qi(q1) 初期コンフィギュレーション点(初期位置)
qg(q4) 目標コンフィギュレーション点(目標位置)
qm(qm1,qm2,qm3・・・) 中間点
qm4,qm5 シフト中間点
DESCRIPTION OF SYMBOLS 1 Smooth path | route production | generation apparatus 2 Operation panel 3 Arithmetic processing apparatus 4 Memory | storage device 31 Path waypoint setting means 32 Path waypoint pair selection means 33 Intermediate point calculation means 34 Intermediate point interference check means 41 Initial position storage means 42 Target position storage means 43 Robot shape storage means 44 Work space storage means 45 Motion path storage means 46 Path halfway point storage means 100, 100 'Robot arm (multi-axis robot arm)
CA Interference area Ob Obstacle q1, q2 Path intermediate point pair q2, q3 Path intermediate point pair q3, q4 Path intermediate point pair qi (q1) Initial configuration point (initial position)
qg (q4) Target configuration point (target position)
qm (qm1, qm2, qm3 ...) Intermediate point qm4, qm5 Shift intermediate point

Claims (4)

演算処理装置および記憶装置からなる制御装置を有し、
初期位置から目標位置まで障害物を回避するように生成された多軸ロボットアームの動作経路を円滑化して円滑経路を再生成する円滑経路生成装置であって、
前記制御装置は、
前記初期位置から前記目標位置までの動作経路を格納する動作経路記憶手段と、
前記動作経路における複数の経路途中点を設定する経路途中点設定手段と、
前記複数の経路途中点を格納する経路途中点記憶手段と、
前記多軸ロボットアームのいずれか1以上の軸について、前記経路途中点のうち対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択する経路途中点対採択手段と、
この経路途中点対採択手段により採択された前記一の経路途中点と他の経路途中点との中間点を、計算式(一の経路途中点の位置データ+他の経路途中点の位置データ)/2から求める中間点演算手段と、
この中間点演算手段により求めた中間点が前記障害物と干渉するかどうかを判断する中間点干渉チェック手段と、を備え、
前記中間点干渉チェック手段により、前記中間点が前記障害物と干渉すると判断された場合には、当該中間点を新経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し、
前記中間点干渉チェック手段により、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一の経路途中点から当該新経路途中点を通って前記他の経路途中点まで進む新動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップを実行することを特徴とする円滑経路生成装置。
Having a control unit comprising an arithmetic processing unit and a storage unit;
A smooth path generating device that smoothes an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position and regenerates the smooth path,
The controller is
Motion path storage means for storing a motion path from the initial position to the target position;
Route halfway point setting means for setting a plurality of route halfway points in the operation route;
Route halfway storage means for storing the plurality of route halfway points;
Route intermediate point pair selection means for selecting one of the route intermediate points as a pair and a route intermediate point pair consisting of another route intermediate point with respect to any one or more axes of the multi-axis robot arm. When,
An intermediate point between the one route intermediate point and the other route intermediate point adopted by the route intermediate point pair selection means is calculated as a formula (position data of one route intermediate point + position data of another route intermediate point). Intermediate point calculation means obtained from / 2,
An intermediate point interference check unit for determining whether the intermediate point obtained by the intermediate point calculation unit interferes with the obstacle, and
If the intermediate point interference check means determines that the intermediate point interferes with the obstacle, the current route is maintained without adopting the intermediate point as a new route halfway point. Perform steps,
When the intermediate point interference check means determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is changed from the one route intermediate point to the relevant point. A smooth route generating apparatus, wherein a smooth route generating step is performed in which a smooth route is regenerated by changing to a new motion route that passes through a new route halfway to the other route halfway.
前記経路途中点対採択手段は、前記経路途中点対を数珠つながりに連続して多数採択し、
前記中間点演算手段は、前記多数の経路途中点対においてそれぞれ多数の中間点を求め、
前記中間点干渉チェック手段は、前記多数の中間点がそれぞれ前記障害物と干渉するかどうかを判断して、
前記円滑経路生成ステップでは、前記中間点干渉チェック手段により、前記多数の中間点のうち当該中間点がそれぞれ前記障害物と干渉すると判断された場合には、当該それぞれの中間点を新経路途中点として採択せずにそれぞれの現在のままの経路途中点対で構成される動作経路を採択する現在経路維持ステップを実行し、
前記多数の中間点のうち当該中間点がそれぞれ前記障害物と干渉しないと判断された場合には、当該それぞれの中間点を新経路途中点として採択し、前記動作経路を当該それぞれの中間点に係る前記一の経路途中点から当該それぞれの新途中点を通って当該それぞれの中間点に係る前記他の経路途中点まで進む新たな動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップを実行することを特徴とする請求項1に記載の円滑経路生成装置。
The route halfway point adopting means adopts a number of the route halfway point pairs successively in a daisy chain,
The intermediate point calculation means obtains a large number of intermediate points in each of the numerous path halfway point pairs,
The intermediate point interference check means determines whether each of the multiple intermediate points interferes with the obstacle,
In the smooth route generation step, when the intermediate point interference check unit determines that the intermediate point of the multiple intermediate points interferes with the obstacle, the intermediate point is determined as a new route intermediate point. Execute a current route maintenance step that adopts an operation route composed of a pair of intermediate points of each current route without adopting as,
If it is determined that the intermediate points do not interfere with the obstacles among the many intermediate points, the intermediate points are adopted as new route intermediate points, and the operation route is set to the respective intermediate points. Smooth path generation that regenerates the smooth path by changing from the one way intermediate point to the new operation route that passes through the new intermediate point to the other route intermediate point of the respective intermediate point The smooth path generation apparatus according to claim 1, wherein the step is executed.
前記制御装置は、前記円滑経路生成ステップの後、前記経路途中点対採択手段により、
前記一の経路途中点および他の経路途中点のうち少なくとも一方が異なる一のシフト経路途中点、および他のシフト経路途中点からなる一対のシフト経路途中点対を採択するシフト経路途中点対採択ステップを実行し、
前記中間点演算手段により、前記シフト経路途中点対採択手段により採択された前記一のシフト経路途中点と他のシフト経路途中点との中間点であるシフト中間点を、計算式(一のシフト経路途中点の位置データ+他のシフト経路途中点の位置データ)/2から求めるシフト中間点演算ステップを実行し、
前記中間点干渉チェック手段により、前記シフト中間点が前記障害物と干渉すると判断された場合には、当該シフト中間点を新たな経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップを実行し、
前記中間点干渉チェック手段により、前記シフト中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一のシフト経路途中点から当該新経路途中点を通って前記他のシフト経路途中点まで進む新動作経路に変更して前記円滑経路を再生成するシフト円滑経路生成ステップを実行し、
このようにして、前記円滑経路生成ステップの後に前記現在経路維持ステップまたは前記シフト円滑経路生成ステップを実行し、前記円滑経路生成ステップから前記現在経路維持ステップまたは前記シフト円滑経路生成ステップを順次繰り返して動作経路を円滑化することを特徴とする請求項1または請求項2に記載の円滑経路生成装置。
The control device, after the smooth route generation step, by the route halfway point selection means,
Adopting a pair of shift path midpoints consisting of one shift path midpoint in which at least one of the one path midpoint and the other path midpoint is different and another shift path midpoint pair is adopted Perform steps,
A shift intermediate point that is an intermediate point between the one shift path intermediate point and the other shift path intermediate point selected by the intermediate point calculation means and the shift path intermediate point pair selection means is calculated by a formula (one shift (2) Shift intermediate point calculation step obtained from the position data of the intermediate point of the route + position data of other intermediate point of the shift route) / 2
If the intermediate point interference check means determines that the shift intermediate point interferes with the obstacle, the current operation route is adopted without adopting the shift intermediate point as a new route intermediate point. Perform current route maintenance step,
When the intermediate point interference check means determines that the shift intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is set as the one shift route intermediate point. To perform a shift smooth path generation step for regenerating the smooth path by changing to a new operation path that travels from the midpoint of the new path to the other midpoint of the shift path,
In this way, the current route maintenance step or the shift smooth route generation step is executed after the smooth route generation step, and the current route maintenance step or the shift smooth route generation step is sequentially repeated from the smooth route generation step. The smooth path generating apparatus according to claim 1 or 2, wherein the operating path is smoothed.
初期位置から目標位置まで障害物を回避するように生成された多軸ロボットアームの動作経路を円滑化して円滑経路を再生成する円滑経路生成方法であって、
演算処理装置および記憶装置からなる制御装置を有する円滑経路生成装置を使用して、
前記演算処理装置を構成する経路途中点設定手段により、前記動作経路における複数の経路途中点を設定する経路途中点設定ステップと、
前記演算処理装置を構成する経路途中点採択手段により、前記多軸ロボットアームのいずれか1以上の軸について、前記経路途中点のうち対となる一の経路途中点、および他の経路途中点からなる経路途中点対を採択する経路途中点対採択ステップと、
前記演算処理装置を構成する中間点演算手段により、前記経路途中点対採択ステップで採択された前記一の経路途中点と他の経路途中点との中間点を、計算式(一の経路途中点の位置データ+他の経路途中点の位置データ)/2から求める中間点演算ステップと、
前記演算処理装置を構成する中間点干渉チェック手段により、前記中間点演算ステップで求めた中間点が前記障害物と干渉するかどうかを判断する中間点干渉チェックステップと、
この中間点干渉チェックステップにより、前記中間点が前記障害物と干渉すると判断された場合には、当該中間点を新経路途中点として採択せずに現在のままの動作経路を採択する現在経路維持ステップと、
前記中間点干渉チェックステップにより、前記中間点が前記障害物と干渉しないと判断された場合には、当該中間点を新経路途中点として採択し、前記動作経路を前記一の経路途中点から当該新経路途中点を通って前記他の経路途中点まで進む新動作経路に変更して前記円滑経路を再生成する円滑経路生成ステップと、
を含むことを特徴とする円滑経路生成方法。
A smooth path generation method for smoothing an operation path of a multi-axis robot arm generated so as to avoid an obstacle from an initial position to a target position and regenerating the smooth path,
Using a smooth path generation device having a control device comprising an arithmetic processing device and a storage device,
A route halfway point setting step for setting a plurality of route halfway points in the operation route by a route halfway point setting means constituting the arithmetic processing unit;
By means of route halfway point adopting means constituting the arithmetic processing unit, from any one or more axes of the multi-axis robot arm from one route halfway point and another route halfway point that are paired among the route halfway points A path halfway point adopting step for adopting a path halfway point pair,
An intermediate point between the one route intermediate point and the other route intermediate point adopted in the route intermediate point pair selection step is calculated by an intermediate point calculating means (one route intermediate point) constituting the arithmetic processing unit. Position data + position data of other route intermediate points) / 2)
An intermediate point interference check step for determining whether the intermediate point obtained in the intermediate point calculation step interferes with the obstacle by the intermediate point interference check means constituting the arithmetic processing unit;
When the intermediate point interference check step determines that the intermediate point interferes with the obstacle, the current route is maintained without adopting the intermediate point as a new route intermediate point. Steps,
When the intermediate point interference check step determines that the intermediate point does not interfere with the obstacle, the intermediate point is adopted as a new route intermediate point, and the operation route is changed from the one route intermediate point to the relevant point. A smooth path generating step of regenerating the smooth path by changing to a new operation path that travels through a new path midpoint to the other path midpoint;
A smooth path generation method comprising:
JP2010203871A 2010-09-13 2010-09-13 Smooth path generation device and smooth path generation method Active JP5361004B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2010203871A JP5361004B2 (en) 2010-09-13 2010-09-13 Smooth path generation device and smooth path generation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2010203871A JP5361004B2 (en) 2010-09-13 2010-09-13 Smooth path generation device and smooth path generation method

Publications (2)

Publication Number Publication Date
JP2012056063A true JP2012056063A (en) 2012-03-22
JP5361004B2 JP5361004B2 (en) 2013-12-04

Family

ID=46053776

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2010203871A Active JP5361004B2 (en) 2010-09-13 2010-09-13 Smooth path generation device and smooth path generation method

Country Status (1)

Country Link
JP (1) JP5361004B2 (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013246553A (en) * 2012-05-24 2013-12-09 Toyota Motor Corp Track planning device, track planning method and track planning program
JP2014073550A (en) * 2012-10-04 2014-04-24 Seiko Epson Corp Path searching method, path searching device, robot control device, robot, and program
JP2015037818A (en) * 2013-08-17 2015-02-26 セイコーエプソン株式会社 Robot, control method for robot, program, and route creation device
DE102015102399B4 (en) * 2014-02-27 2016-08-18 Fanuc Corporation Robot simulation device for generating a movement path of a robot
GB2542892A (en) * 2015-07-28 2017-04-05 Harris Corp Path-optimized manipulator reversing controller
US9925664B2 (en) 2014-02-27 2018-03-27 Fanuc Corporation Robot simulation device for generation motion path of robot
JP6443905B1 (en) * 2017-12-28 2018-12-26 深セン市優必選科技有限公司 Robot motion path planning method, apparatus, storage medium, and terminal device
CN111761582A (en) * 2020-07-08 2020-10-13 浙江大学 Mobile mechanical arm obstacle avoidance planning method based on random sampling
CN111998864A (en) * 2020-08-11 2020-11-27 东风柳州汽车有限公司 Unmanned vehicle local path planning method, device, equipment and storage medium
DE102019128443B4 (en) 2018-10-31 2022-12-22 Fanuc Corporation Automatic program correction facility, automatic program correction method and automatic path generation facility
WO2023007574A1 (en) * 2021-07-27 2023-02-02 三菱電機株式会社 Robot control device and robot control method

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04235606A (en) * 1991-01-11 1992-08-24 Toshiba Corp Method and device for deciding free space connection
JP2000339012A (en) * 1999-05-26 2000-12-08 Yaskawa Electric Corp Method for planning route for global operation route of robot and its controller
JP2001154706A (en) * 1999-11-29 2001-06-08 Japan Atom Energy Res Inst Method for generating route of mobile object
JP2002073130A (en) * 2000-06-13 2002-03-12 Yaskawa Electric Corp Planning method for gross motion path of robot and its controller
JP2004209562A (en) * 2002-12-27 2004-07-29 Honda Motor Co Ltd Mobile robot
JP2006048372A (en) * 2004-08-04 2006-02-16 Toyota Motor Corp Path-planning method
JP2008105132A (en) * 2006-10-25 2008-05-08 Toyota Motor Corp Method and apparatus for producing path of arm in joint space
JP2008269021A (en) * 2007-04-16 2008-11-06 Toyota Motor Corp Method and apparatus for creating path
JP2009032185A (en) * 2007-07-30 2009-02-12 Toyota Motor Corp Moving route acquisition device
JP2010032326A (en) * 2008-07-28 2010-02-12 Toshiba Corp Device and method for work in nuclear reactor

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH04235606A (en) * 1991-01-11 1992-08-24 Toshiba Corp Method and device for deciding free space connection
JP2000339012A (en) * 1999-05-26 2000-12-08 Yaskawa Electric Corp Method for planning route for global operation route of robot and its controller
JP2001154706A (en) * 1999-11-29 2001-06-08 Japan Atom Energy Res Inst Method for generating route of mobile object
JP2002073130A (en) * 2000-06-13 2002-03-12 Yaskawa Electric Corp Planning method for gross motion path of robot and its controller
JP2004209562A (en) * 2002-12-27 2004-07-29 Honda Motor Co Ltd Mobile robot
JP2006048372A (en) * 2004-08-04 2006-02-16 Toyota Motor Corp Path-planning method
JP2008105132A (en) * 2006-10-25 2008-05-08 Toyota Motor Corp Method and apparatus for producing path of arm in joint space
JP2008269021A (en) * 2007-04-16 2008-11-06 Toyota Motor Corp Method and apparatus for creating path
JP2009032185A (en) * 2007-07-30 2009-02-12 Toyota Motor Corp Moving route acquisition device
JP2010032326A (en) * 2008-07-28 2010-02-12 Toshiba Corp Device and method for work in nuclear reactor

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
JPN6013037838; '経路の変形と再探索を併用したオンライン動作再計画' 吉田英一、金広文男、横井一仁、Pierre Gergondet Vol.29 No.8, 20111015, pp.716-725, 日本ロボット学会誌 *
JPN7013002863; Sean Quinlan and Oussama Khatib: 'Elastic bands : Connecting Path Planning and Control' International Conference on Robotics and Automation , 1993, pp.802-807, IEEE *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013246553A (en) * 2012-05-24 2013-12-09 Toyota Motor Corp Track planning device, track planning method and track planning program
JP2014073550A (en) * 2012-10-04 2014-04-24 Seiko Epson Corp Path searching method, path searching device, robot control device, robot, and program
JP2015037818A (en) * 2013-08-17 2015-02-26 セイコーエプソン株式会社 Robot, control method for robot, program, and route creation device
DE102015102399B4 (en) * 2014-02-27 2016-08-18 Fanuc Corporation Robot simulation device for generating a movement path of a robot
US9573273B2 (en) 2014-02-27 2017-02-21 Fanuc Corporation Robot simulation device for generating motion path of robot
US9925664B2 (en) 2014-02-27 2018-03-27 Fanuc Corporation Robot simulation device for generation motion path of robot
GB2542892A (en) * 2015-07-28 2017-04-05 Harris Corp Path-optimized manipulator reversing controller
GB2542892B (en) * 2015-07-28 2020-01-22 Harris Corp Path-optimized manipulator reversing controller
JP2019119041A (en) * 2017-12-28 2019-07-22 深セン市優必選科技股▲ふん▼有限公司Ubtech Pobotics Corp Ltd Robot motion path planning method, apparatus, storage medium, and terminal device
JP6443905B1 (en) * 2017-12-28 2018-12-26 深セン市優必選科技有限公司 Robot motion path planning method, apparatus, storage medium, and terminal device
DE102019128443B4 (en) 2018-10-31 2022-12-22 Fanuc Corporation Automatic program correction facility, automatic program correction method and automatic path generation facility
US11579587B2 (en) 2018-10-31 2023-02-14 Fanuc Corporation Automatic program-correction device, automatic program-correction method, and automatic path-generation device
CN111761582A (en) * 2020-07-08 2020-10-13 浙江大学 Mobile mechanical arm obstacle avoidance planning method based on random sampling
CN111761582B (en) * 2020-07-08 2021-05-18 浙江大学 Mobile mechanical arm obstacle avoidance planning method based on random sampling
CN111998864A (en) * 2020-08-11 2020-11-27 东风柳州汽车有限公司 Unmanned vehicle local path planning method, device, equipment and storage medium
CN111998864B (en) * 2020-08-11 2023-11-07 东风柳州汽车有限公司 Unmanned vehicle local path planning method, device, equipment and storage medium
WO2023007574A1 (en) * 2021-07-27 2023-02-02 三菱電機株式会社 Robot control device and robot control method
JPWO2023007574A1 (en) * 2021-07-27 2023-02-02

Also Published As

Publication number Publication date
JP5361004B2 (en) 2013-12-04

Similar Documents

Publication Publication Date Title
JP5361004B2 (en) Smooth path generation device and smooth path generation method
JP5775279B2 (en) Route generator
US9827675B2 (en) Collision avoidance method, control device, and program
CN108908331B (en) Obstacle avoidance method and system for super-redundant flexible robot and computer storage medium
Zha Optimal pose trajectory planning for robot manipulators
US8483878B2 (en) Method and apparatus for predicting interference between target section of robot and peripheral object
US20150094855A1 (en) Imitation learning method for a multi-axis manipulator
Lee et al. Optimal design and workspace analysis of a mobile welding robot with a 3P3R serial manipulator
US20110172819A1 (en) Manipulator and control method thereof
Haviland et al. NEO: A novel expeditious optimisation algorithm for reactive motion control of manipulators
Korayem et al. Trajectory optimization of nonholonomic mobile manipulators departing to a moving target amidst moving obstacles
JP2009134352A (en) Robot motion path creating device, and robot motion path creating method
JP2016055404A (en) Locus generation method, locus generation device, robot device, program, and recording medium
JP5885003B2 (en) ROBOT TRAVEL RUNNING AREA CHECK METHOD, ROBOT, AND RECORDING MEDIUM
Moldovan et al. Dynamic numerical simulation of the 6-PGK parallel robot manipulator
He et al. Haptic-aided robot path planning based on virtual tele-operation
EP3978203A1 (en) Singularity-free kinematic parameterization of soft robot manipulators
Jiang et al. Obstacle-avoidance path planning based on the improved artificial potential field for a 5 degrees of freedom bending robot
Zhang et al. Kuka youBot arm shortest path planning based on geodesics
Suárez et al. Kinematics, dynamics and evaluation of energy consumption for ABB IRB-140 serial robots in the tracking of a path
Vosniakos et al. Motion coordination for industrial robotic systems with redundant degrees of freedom
JP2021082222A (en) Information processing method, robot system, article manufacturing method, and information processing unit
JP5857803B2 (en) Industrial machine interference determination device, interference determination method, computer program, and recording medium
Zha et al. Trajectory coordination planning and control for robot manipulators in automated material handling and processing
Kim et al. Robotic virtual manipulations of a nuclear hot‐cell digital mock‐up system

Legal Events

Date Code Title Description
A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20111216

A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20121012

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A821

Effective date: 20121012

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

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20130830

R150 Certificate of patent or registration of utility model

Ref document number: 5361004

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

S111 Request for change of ownership or part of ownership

Free format text: JAPANESE INTERMEDIATE CODE: R313117

R350 Written notification of registration of transfer

Free format text: JAPANESE INTERMEDIATE CODE: R350

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250